Added corner collision

2 files changed, 51 insertions, 9 deletions

diff --git a/src/Collision.cpp b/src/Collision.cpp
index 6c301da..91bfc95 100644
--- a/src/Collision.cpp
+++ b/src/Collision.cpp
@@ -84,16 +84,16 @@ bool Collision::testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmm
   vmml::vec3f cvec = v1 + vmml::dot(edge, m-v1)*edge / edge.squared_length() - m;
 
   float a = avec.squared_length();
-  float b = vmml::dot(avec, cvec);
+  float b_2 = vmml::dot(avec, cvec);
   float c = cvec.squared_length() - r*r;
 
-  float rootSq = b*b - a*c;
+  float rootSq = b_2*b_2 - a*c;
   if(rootSq < 0)
     return false;
 
   float root = std::sqrt(rootSq);
 
-  *distance = -(b + root)/a;
+  *distance = -(b_2 + root)/a;
 
   float edgeFact = edge.dot(m + move*(*distance) - v1);
   if(edgeFact < 0 || edgeFact > edge.squared_length())
@@ -105,6 +105,25 @@ bool Collision::testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmm
     return false;
 }
 
+bool Collision::testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
+  float a = move.squared_length();
+  float b_2 = vmml::dot(m-v, move);
+  float c = (m-v).squared_length() - r*r;
+
+  float rootSq = b_2*b_2 - a*c;
+  if(rootSq < 0)
+    return false;
+
+  float root = std::sqrt(rootSq);
+
+  *distance = -(b_2 + root)/a;
+
+  if(*distance > -MathUtil::EPSILON && *distance < 1)
+    return true;
+  else
+    return false;
+}
+
 bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
   float d;
 
@@ -120,20 +139,42 @@ bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmm
     }
   }
 
-  float minEdgeDistance = std::numeric_limits<float>::infinity();
+  bool collision = false;
+  float minDistance;
 
   for(int i = 0; i < 3; ++i) {
     if(testEdge(t.getVertex(i), t.getVertex((i+1)%3), m, r, move, &d)) {
-      if(d < minEdgeDistance) {
-        minEdgeDistance = d;
+      if(!collision || d < minDistance) {
+        minDistance = d;
       }
     }
   }
 
-  if(distance)
-    *distance = minEdgeDistance;
+  if(collision) {
+    if(distance)
+      *distance = minDistance;
 
-  return (minEdgeDistance < 1);
+    return true;
+  }
+
+  collision = false;
+  for(int i = 0; i < 3; ++i) {
+    if(testVertex(t.getVertex(i), m, r, move, &d)) {
+      if(!collision || d < minDistance) {
+        minDistance = d;
+      }
+    }
+  }
+
+  if(collision) {
+    if(distance)
+      *distance = minDistance;
+
+    return true;
+  }
+  else {
+    return false;
+  }
 }
 
 }
diff --git a/src/Collision.h b/src/Collision.h
index 42e5399..df69289 100644
--- a/src/Collision.h
+++ b/src/Collision.h
@@ -33,6 +33,7 @@ class Collision {
     Collision();
 
     static bool testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance);
+    static bool testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance);
 
     static vmml::vec3f projectToEdge(const vmml::vec3f& p, const vmml::vec3f& v1, const vmml::vec3f& v2);
     static vmml::vec3f projectToNearestEdge(const vmml::vec3f& p, const Triangle &t);