From affcc5305bc032b27eb3e50ac222144a0820bfc3 Mon Sep 17 00:00:00 2001
From: William Moberg <william.moberg@live.se>
Date: Tue, 2 Feb 2016 17:19:59 +0100
Subject: [PATCH] Collision affects velocity correctly, no wierd ice cream
 sliding along walls.

---
 src/Engine/Collision/Collision.cpp | 48 +++++++++++++++++++-----------
 1 file changed, 30 insertions(+), 18 deletions(-)

diff --git a/src/Engine/Collision/Collision.cpp b/src/Engine/Collision/Collision.cpp
index edb6c9a9..81949cb5 100644
--- a/src/Engine/Collision/Collision.cpp
+++ b/src/Engine/Collision/Collision.cpp
@@ -344,9 +344,10 @@ constexpr float SlopeConstant(float degrees)
 //An array containing 3 int pairs { 0, 2 }, { 0, 1 }, { 1, 2 }
 constexpr std::array<std::pair<int, int>, 3> dimensionPairs({ std::pair<int, int>(0, 2), std::pair<int, int>(0, 1), std::pair<int, int>(1, 2) });
 
+//TODO: Prefer to move in y - if the resolution is small enough, value in physics comp.
+//TODO: Walking down slopes correctly.
 bool AABBvsTriangle(const AABB& box, 
     const std::array<glm::vec3, 3>& triPos, 
-    const glm::vec3& wantDirection,
     glm::vec3& boxVelocity,
     glm::vec3& outVector)
 {
@@ -354,7 +355,7 @@ bool AABBvsTriangle(const AABB& box,
     //Also, don't check a triangle facing away from the player.
     //Less checks, and we should be able to walk out from models if we are trapped inside.
     glm::vec3 triNormal = glm::cross(triPos[1] - triPos[0], triPos[2] - triPos[0]);
-    if (!vectorHasLength(triNormal) || (glm::dot(triNormal, boxVelocity) > 0) && vectorHasLength(boxVelocity)) {
+    if (!vectorHasLength(triNormal) || (glm::dot(triNormal, boxVelocity) > 0)) {
         return false;
     }
 
@@ -428,40 +429,56 @@ bool AABBvsTriangle(const AABB& box,
     {
         //If we get here, the resolution is along one coordinate axis.
         //set velocity to 0 in that dimension.
+        //projNorm = glm::vec3(0.f);
+        //projNorm[resolveCase] = 1;
+        //ImGui::Text(("Axis " + std::string(projNorm.y > SlopeConstant(45.0f) ? "Ground" : "Slope") + "collision proj=(%f,%f,%f)").c_str(), projNorm.x, projNorm.y, projNorm.z);
         boxVelocity[resolveCase] = 0.f;
         return true;
     }
     case Line:
         projNorm = glm::normalize(outVector);
+        //ImGui::Text(("Line " + std::string(projNorm.y > SlopeConstant(45.0f) ? "Ground" : "Slope") + "collision proj=(%f,%f,%f)").c_str(), projNorm.x, projNorm.y, projNorm.z);
         break;
     case Corner:
         projNorm = triNormal;
+        //ImGui::Text(("Corner " + std::string(projNorm.y > SlopeConstant(45.0f) ? "Ground" : "Slope") + "collision proj=(%f,%f,%f)").c_str(), projNorm.x, projNorm.y, projNorm.z);
         break;
     default:
         break;
     }
 
-    //Project the velocity onto the normal of the hit line/face.
-    //w = v - <v,n>*n, |n|==1.
-    boxVelocity = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
     //If the collision was not on steep wall or similarly (e.g. walking on the ground), do special treatment.
     //Magic value that makes condition correspond to: 
     //if the angle between horizon and the collision surface is less than 45 degrees. 
     if (projNorm.y > SlopeConstant(45.0f)) {
-        //Make sure the player keeps moving in their desired direction, just slower.
-        float len = glm::length2(boxVelocity);
-        if (len > 0.0001f) {
-            boxVelocity = glm::sqrt(len) * wantDirection;
-        }
-
         //Ensure that the player always is moved upwards, instead of sliding down.
-        len = glm::length(outVector);
+        //Also zero the vertical velocity.
+        float len = glm::length(outVector);
         float ang = glm::half_pi<float>() - glm::acos(outVector.y / len);
         if (len > 0.0000001f && ang > 0.0000001f) {
             outVector.x = 0;
             outVector.y = len / glm::sin(ang);
             outVector.z = 0;
+            boxVelocity.y = 0.f;
         }
+    } else if (projNorm.y > 0) {
+        //Enter here if the triangle is a steep slope, and it is not facing downwards.
+        //Project the velocity onto the normal of the hit line/face.
+        //w = v - <v,n>*n, |n|==1.
+        boxVelocity = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
+
+        //Ensure that the player will be pushed in the xz-plane.
+        //glm::vec3 moveDir(outVector);
+        //moveDir.y = 0;
+        //if (vectorHasLength(moveDir)) {
+        //    moveDir = glm::normalize(moveDir);
+        //    float len = glm::length(outVector);
+        //    float dotMoveOut = moveDir.x * outVector.x + moveDir.z * outVector.z;
+        //    float ang = glm::half_pi<float>() - glm::acos(dotMoveOut / len);
+        //    if (len > 0.0000001f && ang > 0.0000001f) {
+        //        outVector = (len / glm::sin(ang)) * moveDir;
+        //    }
+        //}
     }
     return true;
 }
@@ -477,11 +494,6 @@ bool AABBvsTriangles(const AABB& box,
 
     AABB newBox = box;
     outResolutionVector = glm::vec3(0.f);
-    glm::vec3 wantDirection(boxVelocity);
-    wantDirection.y = 0;
-    if (vectorHasLength(wantDirection)) {
-        wantDirection = glm::normalize(wantDirection);
-    }
     for (int i = 0; i < modelIndices.size(); ) {
         std::array<glm::vec3, 3> triVertices = {
             Transform::TransformPoint(modelVertices[modelIndices[i++]].Position, modelMatrix),
@@ -489,7 +501,7 @@ bool AABBvsTriangles(const AABB& box,
             Transform::TransformPoint(modelVertices[modelIndices[i++]].Position, modelMatrix)
         };
         glm::vec3 outVec;
-        if (AABBvsTriangle(newBox, triVertices, wantDirection, boxVelocity, outVec)) {
+        if (AABBvsTriangle(newBox, triVertices, boxVelocity, outVec)) {
             hit = true;
             outResolutionVector += outVec;
             newBox = AABB::FromOriginSize(newBox.Origin() + outVec, newBox.Size());