Added correct edge collision

3 files changed, 61 insertions, 13 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);
+}
+
 }
diff --git a/src/Collision.h b/src/Collision.h
index 635a7d3..42e5399 100644
--- a/src/Collision.h
+++ b/src/Collision.h
@@ -27,11 +27,13 @@ namespace Zoom {
 class Collision {
   public:
     static bool test(const Triangle &t, const MathUtil::Ray &ray, float *distance = 0);
-    static bool test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move);
+    static bool test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance = 0);
 
   private:
     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 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);
 };
diff --git a/src/Game.cpp b/src/Game.cpp
index e3aa72f..f583312 100644
--- a/src/Game.cpp
+++ b/src/Game.cpp
@@ -30,7 +30,7 @@
 
 namespace Zoom {
 
-const float Game::PLAYER_SPEED = 10;
+const float Game::PLAYER_SPEED = 3;
 const float Game::PLAYER_RADIUS = 0.3;
 
 Game::Game() : playerPos(vmml::vec3f::ZERO), playerRotY(vmml::mat4f::IDENTITY), playerRotX(0),
@@ -157,6 +157,10 @@ void Game::run(int delta) {
             nearestDistance = distance;
           }
         }
+        else {
+          // TODO Edge collision
+
+        }
       }
     }