Can walk up "stair-steps". Cleanup WIP.

Better performance in RectanglevsTriangle check.
Added IsOnGround flag and VerticalStepHeight in Physics.
Lotsa commented code saved for historical reasons, remove later.

include/Engine/Collision/Collision.h

@@ -68,6 +68,8 @@ bool AABBvsTriangles(const AABB& box,
     const std::vector<unsigned int>& modelIndices,
     const glm::mat4& modelMatrix,
     glm::vec3& boxVelocity,
+    float verticalStepHeight,
+    bool& isOnGround,
     glm::vec3& outResolutionVector);
 
 //Return true if the boxes are intersecting.

resources/Schema/Components/Physics.xml

@@ -2,4 +2,6 @@
 <Physics xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="Physics.xsd">
 	<Velocity X="0" Y="0" Z="0"/>
 	<Gravity>true</Gravity>
+	<IsOnGround>false</IsOnGround>
+	<VerticalStepHeight>0.33</VerticalStepHeight>
 </Physics>

resources/Schema/Components/Physics.xsd

@@ -13,6 +13,10 @@
 					<xs:annotation><xs:documentation>m/s^2</xs:documentation></xs:annotation>
 				</xs:element>
 				<xs:element name="Gravity" type="t:bool" minOccurs="0"/>
+				<xs:element name="IsOnGround" type="t:bool" minOccurs="0"/>
+				<xs:element name="VerticalStepHeight" type="t:double" minOccurs="0">
+					<xs:annotation><xs:documentation>The largest height of a "stair-step" that can be walked over</xs:documentation></xs:annotation>
+				</xs:element>
 			</xs:all>
 		</xs:complexType>
 	</xs:element>

src/Engine/Collision/Collision.cpp

@@ -238,7 +238,7 @@ inline glm::vec3 signNonZero(const glm::vec3& x)
 {
     glm::vec3 r;
     for (int i = 0; i < 3; ++i) {
-        r[i] = signNonZero(x[i]);
+        r[i] = (float)signNonZero(x[i]);
     }
     return r;
 }
@@ -253,11 +253,11 @@ bool rectangleVsTriangle(const glm::vec2& boxMin,
     const glm::vec2& boxMax,
     const std::array<glm::vec2, 3>& triPos,
     glm::vec2& resolutionDirection,
-    float& resolutionDistance,
+    float& resolutionDistanceSq,
     bool& pushedFromTriNormal)
 {
     pushedFromTriNormal = false;
-    resolutionDistance = INFINITY;
+    resolutionDistanceSq = INFINITY;
     //Project along box normals (coordinate axes, since it's axis-aligned).
     for (int ax = 0; ax < 2; ++ax) {
         float minTri = INFINITY;
@@ -275,9 +275,11 @@ bool rectangleVsTriangle(const glm::vec2& boxMin,
         float leftRes = minTri - boxMax[ax];
         float rightRes = maxTri - boxMin[ax];
         float push = rightRes < -leftRes ? rightRes : leftRes;
-        float absPush = abs(push);
+        float absPushSq = abs(push);
-        if (absPush < resolutionDistance) {
+        absPushSq *= absPushSq;
-            resolutionDistance = absPush;
+
+        if (absPushSq < resolutionDistanceSq) {
+            resolutionDistanceSq = absPushSq;
             resolutionDirection[1 - ax] = 0.f;
             resolutionDirection[ax] = push;
         }
@@ -326,9 +328,11 @@ bool rectangleVsTriangle(const glm::vec2& boxMin,
         float leftRes = minTri - maxBox;
         float rightRes = maxTri - minBox;
         float push = rightRes < -leftRes ? rightRes : leftRes;
-        float absPush = abs(push);
+        float absPushSq = abs(push);
-        if (absPush < resolutionDistance) {
+        absPushSq *= absPushSq;
-            resolutionDistance = absPush;
+        
+        if (absPushSq < resolutionDistanceSq) {
+            resolutionDistanceSq = absPushSq;
             resolutionDirection = push * normal;
             pushedFromTriNormal = true;
         }
@@ -338,26 +342,35 @@ bool rectangleVsTriangle(const glm::vec2& boxMin,
 
 constexpr float SlopeConstant(float degrees)
 {
-    return (1.0 - degrees / 90.f);
+    return (1.0f - degrees / 90.f);
+}
+
+//Returns true if the angle between horizon and the collision surface is less than 45 degrees. 
+constexpr bool FaceIsGround(float faceNormalY)
+{
+    //TODO: Perhaps the 45 degrees could be saved in a component or in the config..
+    return faceNormalY > SlopeConstant(45.0f);
 }
 
 //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& originalBoxVelocity,
+    float verticalStepHeight,
+    bool& isOnGround,
     glm::vec3& boxVelocity,
-    glm::vec3& outVector)
+    glm::vec3& outResolution)
 {
     //Check so we don't have a zero area triangle when calculating the normal.
     //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)) {
+    if (!vectorHasLength(triNormal) || (glm::dot(triNormal, originalBoxVelocity) > 0)) {
         return false;
     }
+    triNormal = glm::normalize(triNormal);
 
     enum BoxTriResolveCase
     {
@@ -366,13 +379,29 @@ bool AABBvsTriangle(const AABB& box,
         ResolveDimZ,
         Line,       //Box edge colliding with triangle line.
         Corner      //Box corner colliding with the triangle face.
-    } resolveCase;
+    };
+    struct Resolution
+    {
+        Resolution()
+            : DistanceSq(INFINITY)
+            , Vector(0.f)
+        {}
+        BoxTriResolveCase Case;
+        float DistanceSq;
+        glm::vec3 Vector;
+    };
+    //The smallest resolution that solves the collision.
+    Resolution resolveShortest;
+    //The smallest resolution that solves the collision, that resolves upwards.
+    Resolution resolveUpwards;
+    //If player stands on the ground and collides with a ground triangle, 
+    //we might step up onto it if the step is small enough.
+    bool canStairStepUp = isOnGround && FaceIsGround(triNormal.y);
 
     const glm::vec3& origin = box.Origin();
     const glm::vec3& half = box.HalfSize();
     const glm::vec3& min = box.MinCorner();
     const glm::vec3& max = box.MaxCorner();
-    float minimumTranslation = INFINITY;
 
     //For each projection in xy-, xz-, and yx-planes.
     for (std::pair<int, int> dim : dimensionPairs) {
@@ -392,21 +421,35 @@ bool AABBvsTriangle(const AABB& box,
         //if projections don't overlap, return false.
         if (!rectangleVsTriangle(boxMin, boxMax, t2D, resolutionVector, resolutionDist, pushedFromTriangleLine)) {
             return false;
-        } else if (resolutionDist < minimumTranslation) {
+        } else {
-            outVector = glm::vec3(0.f);
+            //Overwrite the smallest resolution if this is smaller.
-            outVector[dim.first] = resolutionVector.x;
+            if (resolutionDist < resolveShortest.DistanceSq) {
-            outVector[dim.second] = resolutionVector.y;
+                resolveShortest.Vector = glm::vec3(0.f);
-            minimumTranslation = resolutionDist;
+                resolveShortest.Vector[dim.first] = resolutionVector.x;
-            //If we pushed away from triangle line (edge), or if we 
+                resolveShortest.Vector[dim.second] = resolutionVector.y;
-            //move the player along one coordinate axis (pick the dimension that isn't zero).
+                resolveShortest.DistanceSq = resolutionDist;
-            resolveCase = pushedFromTriangleLine ? Line : static_cast<BoxTriResolveCase>((abs(outVector[dim.first]) < 0.0001f) ? dim.second : dim.first);
+                //If we pushed away from triangle line (edge), or if we 
+                //move the player along one coordinate axis (pick the dimension that isn't zero).
+                resolveShortest.Case = pushedFromTriangleLine ? Line : static_cast<BoxTriResolveCase>((abs(resolveShortest.Vector[dim.first]) < 0.0001f) ? dim.second : dim.first);
+            }
+            //Overwrite the smallest upward resolution if this is smaller, and resolves upwards.
+            constexpr int yAxis = 1;
+            bool resIsUpwardsIn3D = dim.first == yAxis && resolutionVector.x > 0 || dim.second == yAxis && resolutionVector.y > 0;
+            if (canStairStepUp && resIsUpwardsIn3D && resolutionDist < resolveUpwards.DistanceSq) {
+                resolveUpwards.Vector = glm::vec3(0.f);
+                resolveUpwards.Vector[dim.first] = resolutionVector.x;
+                resolveUpwards.Vector[dim.second] = resolutionVector.y;
+                resolveUpwards.DistanceSq = resolutionDist;
+                //If we pushed away from triangle line (edge), or if we 
+                //move the player along one coordinate axis (pick the dimension that isn't zero).
+                resolveUpwards.Case = pushedFromTriangleLine ? Line : static_cast<BoxTriResolveCase>((abs(resolveUpwards.Vector[dim.first]) < 0.0001f) ? dim.second : dim.first);
+            }
         }
     }
 
     //If the triangle does intersect any of the cube diagonals, it will 
     //intersect the cube diagonal that comes
     //closest to being perpendicular to the plane of the triangle.
-    triNormal = glm::normalize(triNormal);
     glm::vec3 diagonal = signNonZero(triNormal) * half;
     //The triangle plane contains all points P in dot(triNormal, P) == dot(triNormal, v0)
     //The diagonal line contains all points P in P = origin + diagonal * t.
@@ -416,24 +459,62 @@ bool AABBvsTriangle(const AABB& box,
         return false;
     }
     glm::vec3 cornerResolution = (1+t) * diagonal;
-    if (glm::length(cornerResolution) < minimumTranslation) {
+    //Overwrite the smallest resolution if this is smaller.
-        outVector = cornerResolution;
+    float lenSq = glm::length2(cornerResolution);
-        resolveCase = Corner;
+    if (lenSq < resolveShortest.DistanceSq) {
+        resolveShortest.Vector = cornerResolution;
+        resolveShortest.Case = Corner;
+    }
+    if (canStairStepUp && cornerResolution.y > 0 && lenSq < resolveUpwards.DistanceSq) {
+        resolveUpwards.Vector = cornerResolution;
+        resolveUpwards.Case = Corner;
+        resolveUpwards.DistanceSq = lenSq;
     }
 
-    glm::vec3 projNorm;
+    //Force the resolution upwards if it is smaller than the threshold verticalStepHeight.
-    switch (resolveCase) {
+    //Else take the shortest resolution.
-    case ResolveDimX:
+    bool takeUp = resolveUpwards.Vector.y > 0 && resolveUpwards.Vector.y < verticalStepHeight;
+    Resolution& bestResolve = takeUp ? resolveUpwards : resolveShortest;
+    outResolution = bestResolve.Vector;
+
+    //TODO: Debug stuff.
+    std::string dbg;
+    switch (bestResolve.Case) {
     case ResolveDimY:
+    case ResolveDimX:
+    case ResolveDimZ:
+        dbg = "Axis";
+        break;
+    case Line:
+        dbg = "Line";
+        break;
+    case Corner:
+        dbg = "Corner";
+        break;
+    default:
+        break;
+    }
+    std::string outs = (takeUp ? "Resolve upwards " : "Resolve normal ") + dbg;
+    //TODO: End debug stuff.
+
+    glm::vec3 projNorm;
+    switch (bestResolve.Case) {
+    case ResolveDimY:
+        boxVelocity.y = 0.f;
+        if (outResolution.y > 0)
+            isOnGround = true;
+    case ResolveDimX:
     case ResolveDimZ:
     {
         //If we get here, the resolution is along one coordinate axis.
-        //set velocity to 0 in that dimension.
+        //set velocity to 0 in y if it is along y-axis.
-        boxVelocity[resolveCase] = 0.f;
+        outs += isOnGround ? " ground" : " air";
+        ImGui::Text(outs.c_str());
+        LOG_INFO(outs.c_str());
         return true;
     }
     case Line:
-        projNorm = glm::normalize(outVector);
+        projNorm = glm::normalize(outResolution);
         break;
     case Corner:
         projNorm = triNormal;
@@ -442,26 +523,64 @@ bool AABBvsTriangle(const AABB& box,
         break;
     }
 
-    //If the collision was not on steep wall or similarly (e.g. walking on the ground), do special treatment.
+    isOnGround = false;
-    //Magic value that makes condition correspond to: 
+    //If the collision was not on steep wall or similarly (e.g. walking on the ground), force resolution in y only.
-    //if the angle between horizon and the collision surface is less than 45 degrees. 
+    if (FaceIsGround(projNorm.y)) {
-    if (projNorm.y > SlopeConstant(45.0f)) {
         //Ensure that the player always is moved upwards, instead of sliding down.
         //Also zero the vertical velocity.
-        float len = glm::length(outVector);
+        float len = glm::length(outResolution);
-        float ang = glm::half_pi<float>() - glm::acos(outVector.y / len);
+        float ang = glm::half_pi<float>() - glm::acos(outResolution.y / len);
         if (len > 0.0000001f && ang > 0.0000001f) {
-            outVector.x = 0;
+            outResolution.x = 0;
-            outVector.y = len / glm::sin(ang);
+            outResolution.y = len / glm::sin(ang);
-            outVector.z = 0;
+            outResolution.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;
+        glm::vec3 projVel = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
+        if (abs(originalBoxVelocity.x) < 0.001f && abs(originalBoxVelocity.z) < 0.001f) {
+            boxVelocity.y = 0.f;
+        } else {
+            boxVelocity.y = std::min(projVel.y, 0.f);
+        }
+
+        isOnGround = true;
+    } else if (projNorm.y > 0) {
+        //Enter here if the triangle is a steep slope, and it is not facing downwards.
+        //Ensure that the player will be pushed in the xz-plane.
+        //glm::vec3 moveDir(outResolution);
+        //moveDir.y = 0;
+        //if (vectorHasLength(moveDir)) {
+        //    moveDir = glm::normalize(moveDir);
+        //    float len = glm::length(outResolution);
+        //    float dotMoveOut = moveDir.x * outResolution.x + moveDir.z * outResolution.z;
+        //    float ang = glm::half_pi<float>() - glm::acos(dotMoveOut / len);
+        //    if (len > 0.0000001f && ang > 0.0000001f) {
+        //        outResolution = (len / glm::sin(ang)) * moveDir;
+        //    }
+        //}
+        //Project the velocity onto the normal of the hit line/face.
+        //w = v - <v,n>*n, |n|==1.
+        //TODO: What do we want..
+#if 0   //Walk up steep walls.
+        glm::vec3 projVel = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
+        boxVelocity.y = std::min(projVel.y, 0.f);
+        isOnGround = true;
+#elif 1 //"ice cream"-effect, air resistance + projected velocity.
+        if (!isOnGround) {
+            boxVelocity = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
+        }
+        isOnGround = false;
+#elif 0 //"ice cream"-effect, ground friction (full control) + projected velocity.
+        if (!isOnGround) {
+            boxVelocity = boxVelocity - glm::dot(boxVelocity, projNorm) * projNorm;
+        }
+        isOnGround = true;
+#endif
     }
+    outs += isOnGround ? " ground" : " air";
+    ImGui::Text(outs.c_str());
+    LOG_INFO(outs.c_str());
     return true;
 }
 
@@ -470,12 +589,16 @@ bool AABBvsTriangles(const AABB& box,
     const std::vector<unsigned int>& modelIndices, 
     const glm::mat4& modelMatrix,
     glm::vec3& boxVelocity,
+    float verticalStepHeight,
+    bool& isOnGround,
     glm::vec3& outResolutionVector)
 {
     bool hit = false;
 
+    bool everHitTheGround = false;
     AABB newBox = box;
     outResolutionVector = glm::vec3(0.f);
+    glm::vec3 originalBoxVelocity(boxVelocity);
     for (int i = 0; i < modelIndices.size(); ) {
         std::array<glm::vec3, 3> triVertices = {
             Transform::TransformPoint(modelVertices[modelIndices[i++]].Position, modelMatrix),
@@ -483,13 +606,20 @@ bool AABBvsTriangles(const AABB& box,
             Transform::TransformPoint(modelVertices[modelIndices[i++]].Position, modelMatrix)
         };
         glm::vec3 outVec;
-        if (AABBvsTriangle(newBox, triVertices, boxVelocity, outVec)) {
+        bool collideWithGround = isOnGround;
+        if (AABBvsTriangle(newBox, triVertices, originalBoxVelocity, verticalStepHeight, collideWithGround, boxVelocity, outVec)) {
             hit = true;
             outResolutionVector += outVec;
             newBox = AABB::FromOriginSize(newBox.Origin() + outVec, newBox.Size());
+            if (collideWithGround) {
+                everHitTheGround = isOnGround = true;
+            }
         }
     }
 
+    if (!everHitTheGround) {
+        isOnGround = false;
+    }
     return hit;
 }
 

src/Engine/Collision/CollisionSystem.cpp

@@ -37,15 +37,33 @@ void CollisionSystem::UpdateComponent(EntityWrapper& entity, ComponentWrapper& c
 
             glm::mat4 modelMatrix = Transform::ModelMatrix(boxB.Entity);
 
-            glm::vec3 newVelocity = (glm::vec3)cPhysics["Velocity"];
+            glm::vec3 inOutVelocity = (glm::vec3)cPhysics["Velocity"];
-            if (Collision::AABBvsTriangles(boxA, model->m_Vertices, model->m_Indices, modelMatrix, newVelocity, resolutionVector)) {
+            bool isOnGround = (bool)cPhysics["IsOnGround"];
-                (glm::vec3&)cTransform["Position"] += resolutionVector;
+            //glm::vec3 gravity = glm::vec3(0, 9.82f * dt, 0);
-                cPhysics["Velocity"] = newVelocity;
+            //TODO: glm::abs(inOutVelocity - gravity) doesn't work, because of velocity projection on ground normal.
-            }
+            //if (!isOnGround || glm::any(glm::greaterThan(glm::abs(inOutVelocity - gravity), glm::vec3(0.0001f)))) {
+                float verticalStepHeight = (float)(double)cPhysics["VerticalStepHeight"];
+                if (Collision::AABBvsTriangles(boxA, model->m_Vertices, model->m_Indices, modelMatrix, inOutVelocity, verticalStepHeight, isOnGround, resolutionVector)) {
+                    glm::vec3 pos = (glm::vec3)cTransform["Position"] + resolutionVector;
+                    //Hack: Force the position to be at discrete values after collision, removes jittering.
+                    //constexpr float discreteValue = 1.0e2f;
+                    //pos = glm::vec3(glm::ivec3(discreteValue * pos)) / discreteValue;
+                    //pos = glm::round(discreteValue * pos) / discreteValue;
+                    (glm::vec3&)cTransform["Position"] = pos;
+                    cPhysics["Velocity"] = inOutVelocity;
+                    (bool)cPhysics["IsOnGround"] = isOnGround;
+                } else {
+                    (bool)cPhysics["IsOnGround"] = false;
+                }
+            //} else {
+            //    (glm::vec3&)cTransform["Position"] -= gravity;
+            //    cPhysics["Velocity"] = glm::vec3(0.f);
+            //}
         } else if (Collision::AABBVsAABB(boxA, boxB, resolutionVector)) {
             //Enter here if boxB has no Model.
             (glm::vec3&)cTransform["Position"] += resolutionVector;
-            if (resolutionVector.y > 0) {
+            (bool)cPhysics["IsOnGround"] = resolutionVector.y > 0;
+            if ((bool)cPhysics["IsOnGround"]){
                 ((glm::vec3&)cPhysics["Velocity"]).y = 0.f;
             }
         }

src/Game/Systems/PlayerMovementSystem.cpp

@@ -50,11 +50,13 @@ void PlayerMovementSystem::Update(double dt)
                 wishSpeed = playerMovementSpeed;
             }
             glm::vec3& velocity = cPhysics["Velocity"];
-            ImGui::Text("velocity: (%f, %f, %f)", velocity.x, velocity.y, velocity.z);
+            bool isOnGround = (bool)cPhysics["IsOnGround"];
+            ImGui::Text(isOnGround ? "On ground" : "In air");
+            ImGui::Text("velocity: (%f, %f, %f) |%f|", velocity.x, velocity.y, velocity.z, glm::length(velocity));
             glm::vec3 groundVelocity(0.f, 0.f, 0.f);
-            groundVelocity.x = glm::dot(velocity, glm::vec3(1.f, 0.f, 0.f));
+            groundVelocity.x = velocity.x;
-            groundVelocity.z = glm::dot(velocity, glm::vec3(0.f, 0.f, 1.f));
+            groundVelocity.z = velocity.z;
-            ImGui::Text("groundVelocity: (%f, %f, %f) |%f|", groundVelocity.x, groundVelocity.y, groundVelocity.z, glm::length(wishDirection));
+            ImGui::Text("groundVelocity: (%f, %f, %f) |%f|", groundVelocity.x, groundVelocity.y, groundVelocity.z, glm::length(groundVelocity));
             ImGui::Text("wishDirection: (%f, %f, %f) |%f|", wishDirection.x, wishDirection.y, wishDirection.z, glm::length(wishDirection));
             float currentSpeedProj = glm::dot(groundVelocity, wishDirection);
             float addSpeed = wishSpeed - currentSpeedProj;
@@ -67,7 +69,7 @@ void PlayerMovementSystem::Update(double dt)
                 ImGui::InputFloat("accel", &accel);
                 static float airAccel = 0.5f;
                 ImGui::InputFloat("airAccel", &airAccel);
-                float actualAccel = (velocity.y != 0) ? airAccel : accel;
+                float actualAccel = isOnGround ? accel : airAccel;
                 static float surfaceFriction = 5.f;
                 ImGui::InputFloat("surfaceFriction", &surfaceFriction);
                 float accelerationSpeed = actualAccel * (float)dt * wishSpeed * surfaceFriction;
@@ -76,7 +78,8 @@ void PlayerMovementSystem::Update(double dt)
                 ImGui::Text("velocity: (%f, %f, %f) |%f|", velocity.x, velocity.y, velocity.z, glm::length(velocity));
             }
 
-            if (controller->Jumping() && !controller->Crouching() && velocity.y == 0.f) {
+            if (controller->Jumping() && !controller->Crouching() && isOnGround) {
+                (bool)cPhysics["IsOnGround"] = false;
                 velocity.y += 4.f;
             }
 
@@ -128,6 +131,7 @@ void PlayerMovementSystem::UpdateComponent(EntityWrapper& entity, ComponentWrapp
 
     ComponentWrapper& cPhysics = entity["Physics"];
     glm::vec3& velocity = cPhysics["Velocity"];
+    bool isOnGround = (bool)cPhysics["IsOnGround"];
 
     // Ground friction
     float speed = glm::length(velocity);
@@ -135,7 +139,7 @@ void PlayerMovementSystem::UpdateComponent(EntityWrapper& entity, ComponentWrapp
     ImGui::InputFloat("groundFriction", &groundFriction);
     static float airFriction = 0.f;
     ImGui::InputFloat("airFriction", &airFriction);
-    float friction = (velocity.y != 0) ? airFriction : groundFriction;
+    float friction = isOnGround ? groundFriction : airFriction;
     if (speed > 0) {
         float drop = speed * friction * (float)dt;
         float multiplier = glm::max(speed - drop, 0.f) / speed;