Optimized finding nearest plane

1 files changed, 9 insertions, 1 deletions

diff --git a/src/Game.cpp b/src/Game.cpp
index 5b61da7..e3aa72f 100644
--- a/src/Game.cpp
+++ b/src/Game.cpp
@@ -137,16 +137,24 @@ void Game::run(int delta) {
     ok = true;
 
     MathUtil::Plane nearestPlane;
+    float nearestDistance;
 
     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();
+        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)) {
-          if(ok || p.distance(playerPos) < nearestPlane.distance(playerPos)) {
+          vmml::vec3f intersection = p.intersection(MathUtil::Ray(playerPos, playerMove));
+          float distance = intersection.squared_distance(playerPos);
+
+          if(ok || distance < nearestDistance) {
             ok = false;
 
             nearestPlane = p;
+            nearestDistance = distance;
           }
         }
       }