Another attempt to fix invalid error bias

Assets/Scenes/SS_Playground.shade

@@ -4,8 +4,8 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 2, z: 5}
+      Position: {x: 0, y: 10, z: 0}
-      Pitch: 0
+      Pitch: -90
       Yaw: 0
       Roll: 0
       Width: 1920
@@ -21,9 +21,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 0, z: 0}
+      Translate: {x: -5.3379097, y: 5, z: -0.258950353}
-      Rotate: {x: -0, y: 0, z: -0}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
-      Scale: {x: 5, y: 0.999998808, z: 5}
+      Scale: {x: 9.99821568, y: 0.999887228, z: 10}
       IsActive: true
     Collider Component:
       Colliders:
@@ -44,7 +44,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.141888797, y: 5, z: 0}
+      Translate: {x: 0, y: 5, z: 0}
       Rotate: {x: -0, y: 0, z: 0}
       Scale: {x: 0.999999702, y: 0.999999702, z: 1}
       IsActive: true
@@ -80,6 +80,259 @@
   Scripts:
     - Type: MovementTest
       Enabled: true
-      forceAmount: 25
+      forceAmount: 50
       torqueAmount: 50
-      rayDistance: 1
+      rayDistance: 1
+      rayYOffset: -0.100000001
+- EID: 131077
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -0.00715482235, y: 0, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 10, y: 0.999998808, z: 10}
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 8
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -0.0296445489, y: 5.5, z: 5.51512575}
+      Rotate: {x: 1.57079601, y: 0, z: -0}
+      Scale: {x: 9.99866009, y: 0.999816775, z: 9.9984293}
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 65543
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 5.43979311, y: 5, z: 0.0634104013}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
+      Scale: {x: 9.99855232, y: 0.999908268, z: 10}
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 131074
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -0.0296445489, y: 5.5, z: -5.14873552}
+      Rotate: {x: 1.57079601, y: 0, z: -0}
+      Scale: {x: 9.99866009, y: 0.999806404, z: 9.99826336}
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 131076
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -2.60022187, y: 5, z: -3.02165651}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 0.999999702, y: 0.999999702, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 65542
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 1.58913052, y: 4.99788427, z: 1.11987209}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 0.999999702, y: 0.999999702, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 9
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 2.48236442, y: 5, z: -3.18853354}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 0.999999702, y: 0.999999702, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 10
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 5, z: 2.37419057}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 0.999999702, y: 0.999999702, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: false
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~

Assets/Scripts/Tests/MovementTest.cs

@@ -28,6 +28,7 @@ public class MovementTest : Script
     public float forceAmount    = 50.0f;
     public float torqueAmount   = 100.0f;
     public float rayDistance    = 1.0f;
+    public float rayYOffset     = -0.1f;
 
     protected override void awake()
     {
@@ -50,9 +51,10 @@ public class MovementTest : Script
         }
 
         Vector3 dirNor = tf.Forward;
-        List<RaycastHit> rayList1 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535);
+        Vector3 offset = new Vector3(0.0f, rayYOffset, 0.0f);
-        List<RaycastHit> rayList2 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535);
+        List<RaycastHit> rayList1 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(22.5f))), rayDistance, false, (ushort)65535);
-        List<RaycastHit> rayList3 = Physics.ColliderRaycast(GameObject, new Ray(Vector3.Zero, dirNor), rayDistance, false, (ushort)65535);
+        List<RaycastHit> rayList2 = Physics.ColliderRaycast(GameObject, new Ray(offset, Vector3.RotateY(dirNor, SHADE.Math.DegreesToRadians(-22.5f))), rayDistance, false, (ushort)65535);
+        List<RaycastHit> rayList3 = Physics.ColliderRaycast(GameObject, new Ray(offset, dirNor), rayDistance, false, (ushort)65535);
     }
 
     protected override void fixedUpdate()
@@ -62,7 +64,6 @@ public class MovementTest : Script
             // Apply force in the direction the box is facing
             // AKA Transform's forward
             body.AddForce(tf.Forward * forceAmount);
-            Debug.Log($"Forward: <{tf.Forward.x}, {tf.Forward.y}, {tf.Forward.z}");
             move = false;
         }
 

SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp

@@ -67,8 +67,8 @@ namespace SHADE
 
   bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    static constexpr float ABSOLUTE_TOLERANCE = 0.01f;
+    static constexpr float ABSOLUTE_TOLERANCE = 0.0005f;
-    static constexpr float RELATIVE_TOLERANCE = 0.95f;
+    static constexpr float RELATIVE_TOLERANCE = 1.002f;
 
     const SHConvexPolyhedron& POLY_A = reinterpret_cast<const SHConvexPolyhedron&>(A);
     const SHConvexPolyhedron& POLY_B = reinterpret_cast<const SHConvexPolyhedron&>(B);
@@ -130,7 +130,7 @@ namespace SHADE
     const SHConvexPolyhedron* incidentPoly  = nullptr;
 
     SHCollisionUtils::FaceQuery minFaceQuery;
-    if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
+    if (FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE + ABSOLUTE_TOLERANCE)
     {
       minFaceQuery  = FACE_QUERY_A;
       referencePoly = &POLY_A;
@@ -157,7 +157,7 @@ namespace SHADE
 
     // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
     // each edge and use that as the contact point.
-    if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
+    if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE + ABSOLUTE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
     {
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -18,6 +18,14 @@
 #include "Physics/SHPhysicsConstants.h"
 #include "Physics/Collision/Narrowphase/SHCollisionUtils.h"
 
+bool IsInfinity(float value)
+{
+  const float MAX_VAL = std::numeric_limits<float>::max();
+  const float MIN_VAL = -MAX_VAL;
+ 
+  return !(MIN_VAL <= value && value <= MAX_VAL);
+} 
+
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -218,6 +226,10 @@ namespace SHADE
         if (std::fabs(contact.penetration) > SHPHYSICS_LINEAR_SLOP)
           errorBias = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP);
 
+        // TODO: This should never occur. Move this check into the ConvexVSConvex to find the error.
+        if (IsInfinity(errorBias))
+          errorBias = 0.0f;
+
         // Warm starting
         // Compute impulses
         SHVec3 impulse = constraint.normal * contact.normalImpulse;
@@ -249,8 +261,6 @@ namespace SHADE
 
   void SHContactSolver::solve() noexcept
   {
-    
-
     for (auto& [key, constraint] : contactConstraints)
     {
       SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
@@ -275,31 +285,6 @@ namespace SHADE
         SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB);
         SHVec3 relativeVelocity = velocityB - velocityA;
 
-        // Get scalar of relative velocity along the normal
-        const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
-
-        // Compute true normal impulse
-        const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
-
-        float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
-        contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
-        newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
-
-        const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
-
-        // Apply impulses
-        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
-        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
-
-        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
-        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
-
-        // Solve normal impulse
-        // Re-compute relative velocity
-        velocityA = vA + SHVec3::Cross(wA, contact.rA);
-        velocityB = vB + SHVec3::Cross(wB, contact.rB);
-        relativeVelocity = velocityB - velocityA;
-
         // Solve tangent impulse
         for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
         {
@@ -325,6 +310,31 @@ namespace SHADE
           vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
           wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
         }
+
+        // Solve normal impulse
+        // Re-compute relative velocity
+        velocityA = vA + SHVec3::Cross(wA, contact.rA);
+        velocityB = vB + SHVec3::Cross(wB, contact.rB);
+        relativeVelocity = velocityB - velocityA;
+
+        // Get scalar of relative velocity along the normal
+        const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
+
+        // Compute true normal impulse
+        const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
+
+        float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
+        contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
+        newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
+
+        const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
+
+        // Apply impulses
+        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
+
+        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
       }
 
       velocityStateA.LinearVelocity   = vA;