3 files changed, 49 insertions, 28 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;
+      }
     }
   }
 
diff --git a/src/Collision.h b/src/Collision.h
index ba987b9..2688435 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, float *distance = 0);
+    static bool test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance = 0, vmml::vec3f *normal = 0);
 
   private:
     Collision();
 
+    static vmml::vec3f projectToEdge(const vmml::vec3f& p, const vmml::vec3f& v1, const vmml::vec3f& v2);
+
     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);
 };
diff --git a/src/Game.cpp b/src/Game.cpp
index aea829f..d13a262 100644
--- a/src/Game.cpp
+++ b/src/Game.cpp
@@ -136,43 +136,32 @@ void Game::run(int delta) {
   while(collision) {
     collision = false;
 
-    MathUtil::Plane nearestPlane;
     float nearestDistance;
+    vmml::vec3f nearestNormal;
 
     for(std::vector<TriangleRecord>::iterator t = triangles.begin(); t != triangles.end(); ++t) {
-      if(Collision::test(t->getTriangle(), playerPos, PLAYER_RADIUS, playerMove)) {
-        vmml::vec3f normal = t->getTriangle().computeNormal();
+      float distance;
+      vmml::vec3f normal;
+
+      if(Collision::test(t->getTriangle(), playerPos, PLAYER_RADIUS, playerMove, &distance, &normal)) {
+        normal.y() = 0;
+
         if(normal.dot(playerMove) >= 0)
           continue;
 
-        MathUtil::Plane p(normal, vmml::dot(normal, t->getTriangle().getVertex(0)+PLAYER_RADIUS*normal));
-        if(p.isInFront(playerPos) || p.contains(playerPos)) {
-          vmml::vec3f intersection = p.intersection(MathUtil::Ray(playerPos, playerMove));
-          float distance = intersection.squared_distance(playerPos);
-
-          if(!collision || distance < nearestDistance) {
-            collision = true;
-
-            nearestPlane = p;
-            nearestDistance = distance;
-          }
-        }
-        else {
-          // TODO Edge collision
+        if(!collision || distance < nearestDistance) {
+          collision = true;
 
+          nearestDistance = distance;
+          nearestNormal = normal;
         }
       }
     }
 
     if(collision) {
-      vmml::vec3f move;
-
-      if(playerMove.dot(nearestPlane.getNormal()) == 0)
-        move = playerMove;
-      else
-        move = nearestPlane.intersection(MathUtil::Ray(playerPos, playerMove)) - playerPos;
+      vmml::vec3f move = playerMove*nearestDistance;
 
-      if(move.dot(origMove) <= 0 && move.squared_length() > MathUtil::EPSILON) {
+      if(move.dot(origMove) <= 0 && move.squared_length() > 0) {
         return;
       }
 
@@ -182,7 +171,8 @@ void Game::run(int delta) {
       playerPos += move;
 
       vmml::vec3f restMove = playerMove - move;
-      playerMove = restMove - nearestPlane.getNormal() * (nearestPlane.getNormal().dot(restMove));
+
+      playerMove = restMove - nearestNormal * (nearestNormal.dot(restMove));
     }
   }