SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -15,6 +15,7 @@
 #include "SHContactManager.h"
 
 // Project Headers
+#include "Math/SHMathHelpers.h"
 #include "Physics/Collision/Narrowphase/SHCollisionDispatch.h"
 
 namespace SHADE
@@ -69,12 +70,12 @@ namespace SHADE
     for (auto manifoldPair = manifolds.begin(); manifoldPair != manifolds.end();)
     {
       // Test collision of every manifold.
-      SHManifold& oldManifold = manifoldPair->second;
+      SHManifold& manifold = manifoldPair->second;
-      SHManifold  newManifold = oldManifold;
+      SHManifold  oldManifold = manifold;
 
-      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(newManifold, *newManifold.A, *newManifold.B);
+      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.A, *manifold.B);
 
-      auto& collisionState = newManifold.state;
+      auto& collisionState = oldManifold.state;
       updateCollisionState(IS_COLLIDING, collisionState);
 
       const bool IS_INVALID = collisionState == SHCollisionState::INVALID;
@@ -84,7 +85,7 @@ namespace SHADE
         continue;
       }
 
-      updateManifold(oldManifold, newManifold);
+      updateManifold(manifold, oldManifold);
       ++manifoldPair;
     }
 
@@ -169,15 +170,56 @@ namespace SHADE
     }
   }
 
-  void SHContactManager::updateManifold(SHManifold& oldManifold, SHManifold& newManifold) noexcept
+  void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept
   {
-    oldManifold.state = newManifold.state;
+    manifold.state = oldManifold.state;
 
     // Early out since exiting a collision does not require an update beyond updating the state
-    if (newManifold.state == SHCollisionState::EXIT)
+    if (manifold.state == SHCollisionState::EXIT)
       return;
 
+    const SHVec3& NORMAL        = manifold.normal;
+    SHVec3& tangent0            = manifold.tangents[0];
+    SHVec3& tangent1            = manifold.tangents[1];
 
+    const SHVec3& OLD_TANGENT_0 = oldManifold.tangents[0];
+    const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1];
+
+    // Compute tangents
+    if (std::fabs(NORMAL.x) >= SHMath::EULER_CONSTANT)
+      tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f };
+    else
+      tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y };
+
+    tangent0 = SHVec3::Normalise(tangent0);
+    tangent1 = SHVec3::Cross(NORMAL, tangent0);
+
+    // Accumulate impulses
+    for (uint32_t i = 0; i < manifold.numContacts; ++i)
+    {
+      SHContact& contact = manifold.contacts[i];
+
+      // Reset impulses
+      contact.normalImpulse = 0.0f;
+      contact.tangentImpulse[0] = contact.tangentImpulse[1] = 0.0f;
+
+      for (uint32_t j = 0; j < oldManifold.numContacts; ++j)
+      {
+        const SHContact& OLD_CONTACT = oldManifold.contacts[j];
+
+        if (OLD_CONTACT.featurePair.key != contact.featurePair.key)
+          continue;
+
+        // If contact features persists, re-project old solution
+        contact.normalImpulse = OLD_CONTACT.normalImpulse;
+
+        const SHVec3 FRICTION_FORCE = OLD_TANGENT_0 * OLD_CONTACT.tangentImpulse[0] + OLD_TANGENT_1 * OLD_CONTACT.tangentImpulse[1];
+        contact.tangentImpulse[0]   = SHVec3::Dot(FRICTION_FORCE, tangent0); 
+        contact.tangentImpulse[1]   = SHVec3::Dot(FRICTION_FORCE, tangent1);
+
+        break;
+      }
+    }
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactManager.h

@@ -95,8 +95,8 @@ namespace SHADE
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    static void updateCollisionState (bool isColliding, SHCollisionState& state)        noexcept;
+    static void updateCollisionState (bool isColliding, SHCollisionState& state)            noexcept;
-    static void updateManifold       (SHManifold& oldManifold, SHManifold& newManifold) noexcept;
+    static void updateManifold       (SHManifold& manifold, const SHManifold& newManifold)  noexcept;
 
     // Removal Helpers