1 files changed, 31 insertions, 2 deletions
diff --git a/src/Collision.cpp b/src/Collision.cpp
index 0161d5d..21ef083 100644
--- a/src/Collision.cpp
+++ b/src/Collision.cpp
@@ -50,6 +50,20 @@ bool Collision::test(const Triangle &t, const MathUtil::Ray &ray, float *distanc
   return true;
 }
 
+vmml::vec3f Collision::projectToEdge(const vmml::vec3f& p, const vmml::vec3f& v1, const vmml::vec3f& v2) {
+  vmml::vec3f pVec = p - v1;
+  vmml::vec3f edge = v2 - v1;
+
+  float lengthSq = edge.squared_length();
+  float edgeProj = vmml::dot(edge, pVec);
+
+  if(edgeProj < 0) return v1;
+  if(edgeProj > lengthSq) return v2;
+
+  return v1 + (edgeProj/lengthSq)*edge;
+}
+
+
 bool Collision::testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
   vmml::vec3f edge = v2 - v1;
 
@@ -116,16 +130,19 @@ bool Collision::testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r,
   return true;
 }
 
-bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
+bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance, vmml::vec3f *normal) {
   if(move.squared_length() == 0)
     return false;
 
   float d;
 
-  if(test(t, MathUtil::Ray(m - r*t.computeNormal(), move), &d) && d > -MathUtil::EPSILON) {
+  vmml::vec3f triangleNormal = t.computeNormal();
+  if(test(t, MathUtil::Ray(m - r*triangleNormal, move), &d) && d > -MathUtil::EPSILON) {
     if(d < 1) {
       if(distance)
         *distance = d;
+      if(normal)
+        *normal = triangleNormal;
 
       return true;
     }
@@ -142,6 +159,12 @@ bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmm
       if(!collision || d < minDistance) {
         collision = true;
         minDistance = d;
+
+        if(normal) {
+          vmml::vec3f p = m + move*d;
+
+          *normal = (p - projectToEdge(p, t.getVertex(i), t.getVertex((i+1)%3)))/r;
+        }
       }
     }
   }
@@ -160,6 +183,12 @@ bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmm
         collision = true;
         minDistance = d;
       }
+
+      if(normal) {
+        vmml::vec3f p = m + move*d;
+
+        *normal = (p - t.getVertex(i))/r;
+      }
     }
   }