1 files changed, 53 insertions, 11 deletions
diff --git a/src/Collision.cpp b/src/Collision.cpp
index 7c72922..6c301da 100644
--- a/src/Collision.cpp
+++ b/src/Collision.cpp
@@ -18,6 +18,8 @@
  */
 
 #include "Collision.h"
+#include <cmath>
+#include <limits>
 
 namespace Zoom {
 
@@ -75,23 +77,63 @@ bool Collision::test(const Triangle &t, const MathUtil::Ray &ray, float *distanc
   return true;
 }
 
-bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move) {
-  float distance;
+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;
 
-  if(test(t, MathUtil::Ray(m - r*t.computeNormal(), move), &distance) && distance > -MathUtil::EPSILON) {
-    if(distance < 1)
-      return true;
-    else
-      return false;
-  }
+  vmml::vec3f avec = vmml::dot(edge, move)*edge / edge.squared_length() - move;
+  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 c = cvec.squared_length() - r*r;
+
+  float rootSq = b*b - a*c;
+  if(rootSq < 0)
+    return false;
+
+  float root = std::sqrt(rootSq);
+
+  *distance = -(b + root)/a;
 
-  vmml::vec3f collisionPoint = projectToNearestEdge(m, t);
-  vmml::vec3f movedPoint = projectToEdge(m, collisionPoint, collisionPoint - move);
+  float edgeFact = edge.dot(m + move*(*distance) - v1);
+  if(edgeFact < 0 || edgeFact > edge.squared_length())
+    return false;
 
-  if(m.squared_distance(movedPoint) < r*r)
+  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;
+
+  if(test(t, MathUtil::Ray(m - r*t.computeNormal(), move), &d) && d > -MathUtil::EPSILON) {
+    if(distance)
+      *distance = d;
+
+    if(d < 1) {
+      return true;
+    }
+    else {
+      return false;
+    }
+  }
+
+  float minEdgeDistance = std::numeric_limits<float>::infinity();
+
+  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(distance)
+    *distance = minEdgeDistance;
+
+  return (minEdgeDistance < 1);
+}
+
 }