Optimized collision detection

1 files changed, 37 insertions, 15 deletions

diff --git a/src/Collision.cpp b/src/Collision.cpp
index e552643..0161d5d 100644
--- a/src/Collision.cpp
+++ b/src/Collision.cpp
@@ -64,18 +64,31 @@ bool Collision::testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmm
   if(rootSq < 0)
     return false;
 
-  float root = std::sqrt(rootSq);
+  float minRoot = -a - b_2;
+  float maxRoot = a*MathUtil::EPSILON - b_2;
 
-  *distance = -(b_2 + root)/a;
+  if(minRoot < 0)
+    minRoot = 0;
+  if(maxRoot < 0)
+    return false;
 
-  float edgeFact = edge.dot(m + move*(*distance) - v1);
-  if(edgeFact < 0 || edgeFact > edge.squared_length())
+  if(rootSq < minRoot*minRoot || rootSq >= maxRoot*maxRoot)
     return false;
 
-  if(*distance > -MathUtil::EPSILON && *distance < 1)
-    return true;
-  else
+  minRoot = a*(edge.dot(m - v1) - edge.squared_length())/edge.dot(move) - b_2;
+  maxRoot = a*edge.dot(m - v1)/edge.dot(move) - b_2;
+
+  if(minRoot < 0)
+    minRoot = 0;
+  if(maxRoot < 0)
+    return false;
+
+  if(rootSq < minRoot*minRoot || rootSq > maxRoot*maxRoot)
     return false;
+
+  *distance = -(b_2 + std::sqrt(rootSq))/a;
+
+  return true;
 }
 
 bool Collision::testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
@@ -87,24 +100,33 @@ bool Collision::testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r,
   if(rootSq < 0)
     return false;
 
-  float root = std::sqrt(rootSq);
+  float minRoot = -a - b_2;
+  float maxRoot = a*MathUtil::EPSILON - b_2;
 
-  *distance = -(b_2 + root)/a;
+  if(minRoot < 0)
+    minRoot = 0;
+  if(maxRoot < 0)
+    return false;
 
-  if(*distance > -MathUtil::EPSILON && *distance < 1)
-    return true;
-  else
+  if(rootSq <= minRoot*minRoot || rootSq >= maxRoot*maxRoot)
     return false;
+
+  *distance = -(b_2 + std::sqrt(rootSq))/a;
+
+  return true;
 }
 
 bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
+  if(move.squared_length() == 0)
+    return false;
+
   float d;
 
   if(test(t, MathUtil::Ray(m - r*t.computeNormal(), move), &d) && d > -MathUtil::EPSILON) {
-    if(distance)
-      *distance = d;
-
     if(d < 1) {
+      if(distance)
+        *distance = d;
+
       return true;
     }
     else {