Fixed sequential impulses

Assets/Scenes/PhysicsSandbox.shade

@@ -4,7 +4,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 3, z: 0}
+      Translate: {x: 0.186280191, y: 4.3224473, z: 0}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true

SHADE_Engine/src/Math/Geometry/SHSphere.cpp

@@ -115,7 +115,7 @@ namespace SHADE
 
   bool SHSphere::Contains(const SHSphere& rhs) const noexcept
   {
-    return BoundingSphere::Contains(rhs);
+    return BoundingSphere::Contains(rhs) == CONTAINS;
   }
 
   float SHSphere::Volume() const noexcept

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.cpp

@@ -27,7 +27,7 @@ namespace SHADE
   SHCollisionShape::SHCollisionShape(SHCollisionShapeID id, Type colliderType)
   : id              { id }
   , flags           { 0 }
-  , parentTransform { nullptr }
+  , collider        { nullptr }
   , collisionTag    { SHCollisionTagMatrix::GetTag(0) }
   {
     flags |= 1U << SHUtilities::ConvertEnum(colliderType);
@@ -135,11 +135,6 @@ namespace SHADE
     material = newMaterial;
   }
 
-  void SHCollisionShape::SetParentTransform(SHTransform& parentTF) noexcept
-  {
-    parentTransform = &parentTF;
-  }
-
   void SHCollisionShape::SetPositionOffset(const SHVec3& posOffset) noexcept
   {
     transform.position = posOffset;

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h

@@ -22,6 +22,12 @@
 
 namespace SHADE
 {
+  /*-----------------------------------------------------------------------------------*/
+  /* Forward Declarations                                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  class SHRigidBody;
+
   /*-----------------------------------------------------------------------------------*/
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
@@ -38,6 +44,7 @@ namespace SHADE
     friend class  SHColliderComponent;
     friend class  SHCollisionShapeFactory;
     friend class  SHCollisionSpace;
+    friend struct SHManifold;
 
   public:
     /*---------------------------------------------------------------------------------*/
@@ -108,8 +115,6 @@ namespace SHADE
     void  SetDensity          (float density)                         noexcept;
     void  SetMaterial         (const SHPhysicsMaterial& newMaterial)  noexcept;
 
-    void  SetParentTransform  (SHTransform& parentTF)                 noexcept;
-
     void  SetPositionOffset   (const SHVec3& posOffset)               noexcept;
     void  SetRotationOffset   (const SHVec3& rotOffset)               noexcept;
 
@@ -137,7 +142,7 @@ namespace SHADE
 
     SHPhysicsMaterial   material;
 
-    SHTransform*        parentTransform;
+    SHCollider*         collider;     // The collider it belongs to.
     SHTransform         transform;
 
     // Needed for conversion to euler angles

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp

@@ -16,6 +16,7 @@
 // Project Headers
 #include "Math/SHMathHelpers.h"
 #include "Math/SHMatrix.h"
+#include "Physics/Collision/SHCollider.h"
 
 namespace SHADE
 {
@@ -38,7 +39,7 @@ namespace SHADE
   {
 
     material        = rhs.material;
-    parentTransform = rhs.parentTransform;
+    collider        = rhs.collider;
     transform       = rhs.transform;
     rotationOffset  = rhs.rotationOffset;
     flags           = rhs.flags;
@@ -54,7 +55,7 @@ namespace SHADE
   {
 
     material        = rhs.material;
-    parentTransform = rhs.parentTransform;
+    collider        = rhs.collider;
     transform       = rhs.transform;
     rotationOffset  = rhs.rotationOffset;
     flags           = rhs.flags;
@@ -75,7 +76,7 @@ namespace SHADE
 
     id              = rhs.id;
     material        = rhs.material;
-    parentTransform = rhs.parentTransform;
+    collider        = rhs.collider;
     transform       = rhs.transform;
     rotationOffset  = rhs.rotationOffset;
     flags           = rhs.flags;
@@ -101,7 +102,7 @@ namespace SHADE
 
     id              = rhs.id;
     material        = rhs.material;
-    parentTransform = rhs.parentTransform;
+    collider        = rhs.collider;
     transform       = rhs.transform;
     rotationOffset  = rhs.rotationOffset;
     flags           = rhs.flags;
@@ -179,12 +180,14 @@ namespace SHADE
 
   void SHSphereCollisionShape::ComputeTransforms() noexcept
   {
-    const float SPHERE_SCALE = std::fabs(SHMath::Max({ parentTransform->scale.x, parentTransform->scale.y, parentTransform->scale.z }));
+    const SHTransform& PARENT_TRANSFORM = collider->GetTransform();
+
+    const float SPHERE_SCALE = std::fabs(SHMath::Max({ PARENT_TRANSFORM.scale.x, PARENT_TRANSFORM.scale.y, PARENT_TRANSFORM.scale.z }));
     SetScale(SPHERE_SCALE);
 
     // Recompute center
-    const SHQuaternion  FINAL_ROT  = parentTransform->orientation * transform.orientation;
+    const SHQuaternion  FINAL_ROT  = PARENT_TRANSFORM.orientation * transform.orientation;
-    const SHMatrix      TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(parentTransform->position);
+    const SHMatrix      TRS        = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(PARENT_TRANSFORM.position);
 
     Center = SHVec3::Transform(transform.position, TRS);
   }
@@ -212,7 +215,8 @@ namespace SHADE
 
   SHMatrix SHSphereCollisionShape::ComputeWorldTransform() const noexcept
   {
-    const SHQuaternion ROTATION = parentTransform->orientation * transform.orientation;
+    const SHTransform& PARENT_TRANSFORM = collider->GetTransform();
+    const SHQuaternion ROTATION = PARENT_TRANSFORM.orientation * transform.orientation;
     const SHVec3 SCALE{ Radius };
 
     return SHMatrix::Transform

SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h

@@ -49,7 +49,7 @@ namespace SHADE
       uint8_t typeB;
     };
 
-    uint32_t key = 0;
+    uint32_t key = std::numeric_limits<uint32_t>::max();
   };
 
   /**

SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.h

@@ -49,8 +49,11 @@ namespace SHADE
     // We only need 4 contact points to build a stable manifold.
     static constexpr int MAX_NUM_CONTACTS = 4;
 
-    SHCollisionShape* A;
+    SHCollisionShape* shapeA;
-    SHCollisionShape* B;
+    SHCollisionShape* shapeB;
+
+    SHRigidBody*      bodyA       = nullptr;
+    SHRigidBody*      bodyB       = nullptr;
 
     uint32_t          numContacts = 0;
     SHCollisionState  state       = SHCollisionState::INVALID;

SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp

@@ -20,13 +20,18 @@ namespace SHADE
   /*-----------------------------------------------------------------------------------*/
 
   inline SHManifold::SHManifold(SHCollisionShape* a, SHCollisionShape* b) noexcept
-  : A { a }
+  : shapeA  { a }
-  , B { b }
+  , shapeB  { b }
-  {}
+  {
+    bodyA = shapeA->collider->rigidBody;
+    bodyB = shapeB->collider->rigidBody;
+  }
 
   inline SHManifold::SHManifold(const SHManifold& rhs) noexcept
-  : A           { rhs.A }
+  : shapeA      { rhs.shapeA }
-  , B           { rhs.B }
+  , shapeB      { rhs.shapeB }
+  , bodyA       { rhs.bodyA }
+  , bodyB       { rhs.bodyB }
   , numContacts { rhs.numContacts }
   , state       { rhs.state }
   , normal      { rhs.normal }
@@ -39,8 +44,10 @@ namespace SHADE
   }
 
   inline SHManifold::SHManifold(SHManifold&& rhs) noexcept
-  : A           { rhs.A }
+  : shapeA      { rhs.shapeA }
-  , B           { rhs.B }
+  , shapeB      { rhs.shapeB }
+  , bodyA       { rhs.bodyA }
+  , bodyB       { rhs.bodyB }
   , numContacts { rhs.numContacts }
   , state       { rhs.state }
   , normal      { rhs.normal }
@@ -61,8 +68,10 @@ namespace SHADE
     if (this == &rhs)
       return *this;
 
-    A           = rhs.A;
+    shapeA      = rhs.shapeA;
-    B           = rhs.B;
+    shapeB      = rhs.shapeB;
+    bodyA       = rhs.bodyA;
+    bodyB       = rhs.bodyB;
     numContacts = rhs.numContacts;
     state       = rhs.state;
     normal      = rhs.normal;
@@ -78,8 +87,10 @@ namespace SHADE
 
   inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept
   {
-    A           = rhs.A;
+    shapeA      = rhs.shapeA;
-    B           = rhs.B;
+    shapeB      = rhs.shapeB;
+    bodyA       = rhs.bodyA;
+    bodyB       = rhs.bodyB;
     numContacts = rhs.numContacts;
     state       = rhs.state;
     normal      = rhs.normal;

SHADE_Engine/src/Physics/Collision/SHCollider.cpp

@@ -307,7 +307,7 @@ namespace SHADE
     SHSphereCollisionShape* sphere = shapeFactory->CreateSphere(NEW_SHAPE_ID, SPHERE_CREATE_INFO);
 
     // Set offsets
-    sphere->SetParentTransform(transform);
+    sphere->collider = this;
     sphere->SetPositionOffset(posOffset);
     sphere->SetRotationOffset(rotOffset);
 

SHADE_Engine/src/Physics/Collision/SHCollider.h

@@ -41,6 +41,7 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     friend class  SHCollisionSpace;
+    friend struct SHManifold;
 
   public:
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Dynamics/Constraints/SHContactConstraint.h

@@ -28,10 +28,7 @@ namespace SHADE
 
     // Use the entity IDs to map resolved constraints back to the bodies
 
-    EntityID  idA         = MAX_EID;
+    SHCollisionKey key;
-    EntityID  idB         = MAX_EID;
-
-    uint32_t  numContacts = 0;
 
     // Material Data
 
@@ -52,6 +49,8 @@ namespace SHADE
     SHVec3    normal;
     SHVec3    tangents[SHContact::NUM_TANGENTS];
     SHContact contacts[SHManifold::MAX_NUM_CONTACTS];
+
+    uint32_t  numContacts = 0;
   };
 } // namespace SHADE
 

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -83,6 +83,42 @@ namespace SHADE
   /* Public Member Functions Definitions                                               */
   /*-----------------------------------------------------------------------------------*/
 
+  void SHContactManager::AddTrigger(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+  {
+    // If id not found, emplace new trigger.
+    auto trigger = triggers.find(key);
+    if (trigger == triggers.end())
+      triggers.emplace(key, Trigger{ A, B, SHCollisionState::INVALID });
+  }
+
+  void SHContactManager::AddManifold(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+  {
+    // If id not found, emplace new manifold
+    auto manifold = manifolds.find(key);
+    if (manifold == manifolds.end())
+      manifolds.emplace(key, SHManifold{ A, B });
+  }
+
+  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid) noexcept
+  {
+    removeInvalidObject(triggers, eid);
+  }
+
+  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid, uint32_t shapeIndex) noexcept
+  {
+    removeInvalidObject(triggers, eid, shapeIndex);
+  }
+
+  void SHContactManager::RemoveInvalidatedManifold(EntityID eid) noexcept
+  {
+    removeInvalidObject(manifolds, eid);
+  }
+
+  void SHContactManager::RemoveInvalidatedManifold(EntityID eid, uint32_t shapeIndex) noexcept
+  {
+    removeInvalidObject(manifolds, eid, shapeIndex);
+  }
+
   void SHContactManager::Update() noexcept
   {
     // Clear expired or invalid collisions. If not, test collision.
@@ -92,9 +128,9 @@ namespace SHADE
       SHManifold& manifold = manifoldPair->second;
       SHManifold  oldManifold = manifold;
 
-      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.A, *manifold.B);
+      const bool IS_COLLIDING = SHCollisionDispatcher::Collide(manifold, *manifold.shapeA, *manifold.shapeB);
 
-      auto& collisionState = oldManifold.state;
+      auto& collisionState = manifold.state;
       updateCollisionState(IS_COLLIDING, collisionState);
 
       const bool IS_INVALID = collisionState == SHCollisionState::INVALID;
@@ -128,41 +164,45 @@ namespace SHADE
     }
   }
 
-  void SHContactManager::AddTrigger(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+  void SHContactManager::SolveCollisions(int numIterations, float dt) noexcept
   {
-    // If id not found, emplace new trigger.
+    static constexpr int SIZE_CONTACTS = sizeof(SHContact) * SHManifold::MAX_NUM_CONTACTS;
-    auto trigger = triggers.find(key);
+
-    if (trigger == triggers.end())
+    // Build constraints
-      triggers.emplace(key, Trigger{ A, B, SHCollisionState::INVALID }).first;
+    for (auto& [key, manifold] : manifolds)
+      contactSolver.AddContact(key, manifold);
+
+    // Solve contacts
+    contactSolver.SolveContacts(numIterations, dt);
+
+    // Map impulses back to manifolds
+    const auto& CONTACT_CONSTRAINTS = contactSolver.GetContantConstraints();
+    const auto& VELOCITY_STATES     = contactSolver.GetVelocities();
+
+    for (auto& [key, contactConstraint] : CONTACT_CONSTRAINTS)
+    {
+      SHManifold& manifold = manifolds.find(key)->second;
+
+      for (uint32_t i = 0; i < contactConstraint.numContacts; ++i)
+        manifold.contacts[i] = contactConstraint.contacts[i];
+
+      // Assign velocities back to the bodies
+      SHRigidBody* bodyA = manifold.bodyA;
+      SHRigidBody* bodyB = manifold.bodyB;
+
+      const auto&   STATE_A = VELOCITY_STATES.find(key.GetEntityA())->second;
+      const auto&   STATE_B = VELOCITY_STATES.find(key.GetEntityB())->second;
+
+      bodyA->SetLinearVelocity(STATE_A.linearVelocity);
+      bodyB->SetLinearVelocity(STATE_B.linearVelocity);
+
+      bodyA->SetAngularVelocity(STATE_A.angularVelocity);
+      bodyB->SetAngularVelocity(STATE_B.angularVelocity);
     }
 
-  void SHContactManager::AddManifold(const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept
+    contactSolver.Reset();
-  {
-    // If id not found, emplace new manifold
-    auto manifold = manifolds.find(key);
-    if (manifold == manifolds.end())
-      manifolds.emplace(key, SHManifold{ A, B }).first;
   }
 
-  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid) noexcept
-  {
-    removeInvalidObject(triggers, eid);
-  }
-
-  void SHContactManager::RemoveInvalidatedTrigger(EntityID eid, uint32_t shapeIndex) noexcept
-  {
-    removeInvalidObject(triggers, eid, shapeIndex);
-  }
-
-  void SHContactManager::RemoveInvalidatedManifold(EntityID eid) noexcept
-  {
-    removeInvalidObject(manifolds, eid);
-  }
-
-  void SHContactManager::RemoveInvalidatedManifold(EntityID eid, uint32_t shapeIndex) noexcept
-  {
-    removeInvalidObject(manifolds, eid, shapeIndex);
-  }
 
   /*-----------------------------------------------------------------------------------*/
   /* Private Member Functions Definitions                                              */
@@ -191,8 +231,6 @@ namespace SHADE
 
   void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept
   {
-    manifold.state = oldManifold.state;
-
     // Early out since exiting a collision does not require an update beyond updating the state
     if (manifold.state == SHCollisionState::EXIT)
       return;

SHADE_Engine/src/Physics/Dynamics/SHContactManager.h

@@ -16,6 +16,7 @@
 #include "Physics/Collision/Contacts/SHCollisionEvents.h"
 #include "Physics/Collision/Contacts/SHCollisionKey.h"
 #include "Physics/Collision/Contacts/SHManifold.h"
+#include "SHContactSolver.h"
 
 namespace SHADE
 {
@@ -64,14 +65,6 @@ namespace SHADE
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    /**
-     * @brief
-     * Removes any invalidated contacts and triggers, then performs narrowphase collision
-     * detection on existing triggers and manifolds.
-     * @return 
-    */
-    void Update                     () noexcept;
-
     void AddTrigger                 (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept;
     void AddManifold                (const SHCollisionKey& key, SHCollisionShape* A, SHCollisionShape* B) noexcept;
 
@@ -80,6 +73,20 @@ namespace SHADE
     void RemoveInvalidatedManifold  (EntityID eid)                      noexcept;
     void RemoveInvalidatedManifold  (EntityID eid, uint32_t shapeIndex) noexcept;
 
+    /**
+     * @brief
+     * Removes any invalidated contacts and triggers, then performs narrowphase collision
+     * detection on existing triggers and manifolds.
+    */
+    void Update                     () noexcept;
+
+    /**
+     * @brief
+     * Builds contact constraints and solves them. Results are stored in the corresponding
+     * manifolds abiding by the sequential impulse method.
+    */
+    void SolveCollisions            (int numIterations, float dt) noexcept;
+
   private:
     /*---------------------------------------------------------------------------------*/
     /* Type Definitions                                                                */
@@ -103,6 +110,8 @@ namespace SHADE
     Manifolds       manifolds;
     Triggers        triggers;
 
+    SHContactSolver contactSolver;
+
     /*---------------------------------------------------------------------------------*/
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -36,40 +36,39 @@ namespace SHADE
   /* Public Member Functions Definitions                                               */
   /*-----------------------------------------------------------------------------------*/
 
-  void SHContactSolver::AddContact(const SHManifold& manifold, const SHRigidBody* rigidBodyA, const SHRigidBody* rigidBodyB) noexcept
+  void SHContactSolver::AddContact(const SHCollisionKey& key, const SHManifold& manifold) noexcept
   {
-    SHContactConstraint& newConstraint = contactConstraints.emplace_back(SHContactConstraint{});
+    SHContactConstraint& newConstraint = contactConstraints.emplace(key, SHContactConstraint{}).first->second;
 
-    const auto* SHAPE_A = manifold.A;
+    const auto* SHAPE_A = manifold.shapeA;
-    const auto* SHAPE_B = manifold.B;
+    const auto* SHAPE_B = manifold.shapeB;
 
-    newConstraint.idA = SHAPE_A->GetEntityID();
+    const auto* BODY_A  = manifold.bodyA;
-    newConstraint.idB = SHAPE_B->GetEntityID();
+    const auto* BODY_B  = manifold.bodyB;
 
     // Add velocities if it doesn't already exist
-    velocityStates.emplace(newConstraint.idA, VelocityState{ rigidBodyA->linearVelocity, rigidBodyB->angularVelocity });
+    velocityStates.emplace(key.GetEntityA(), VelocityState{ BODY_A->linearVelocity, BODY_A->angularVelocity });
-    velocityStates.emplace(newConstraint.idB, VelocityState{ rigidBodyB->linearVelocity, rigidBodyB->angularVelocity });
+    velocityStates.emplace(key.GetEntityB(), VelocityState{ BODY_B->linearVelocity, BODY_B->angularVelocity });
 
     // Mix friction & restitution
-    const float FRICTION_A    = manifold.A->GetFriction();
+    const float FRICTION_A    = SHAPE_A->GetFriction();
-    const float RESTITUTION_A = manifold.A->GetBounciness();
+    const float RESTITUTION_A = SHAPE_A->GetBounciness();
 
-    const float FRICTION_B    = manifold.B->GetFriction();
+    const float FRICTION_B    = SHAPE_B->GetFriction();
-    const float RESTITUTION_B = manifold.B->GetBounciness();
+    const float RESTITUTION_B = SHAPE_B->GetBounciness();
 
     newConstraint.friction    = std::sqrtf(FRICTION_A * FRICTION_B);
     newConstraint.restitution = std::max(RESTITUTION_A, RESTITUTION_B);
 
     // Mass data
 
-    newConstraint.invMassA       = rigidBodyA->invMass;
+    newConstraint.invMassA       = BODY_A->invMass;
-    newConstraint.invMassB       = rigidBodyB->invMass;
+    newConstraint.invInertiaA    = BODY_A->worldInvInertia;
+    newConstraint.centerOfMassA  = BODY_A->worldCentroid;
 
-    newConstraint.invInertiaA    = rigidBodyA->worldInvInertia;
+    newConstraint.invMassB       = BODY_B->invMass;
-    newConstraint.invInertiaB    = rigidBodyB->worldInvInertia;
+    newConstraint.invInertiaB    = BODY_B->worldInvInertia;
-
+    newConstraint.centerOfMassB  = BODY_B->worldCentroid;
-    newConstraint.centerOfMassA  = rigidBodyA->worldCentroid;
-    newConstraint.centerOfMassB  = rigidBodyB->worldCentroid;
 
     // Collision data
 
@@ -91,8 +90,9 @@ namespace SHADE
     }
   }
 
-  void SHContactSolver::ClearContacts() noexcept
+  void SHContactSolver::Reset() noexcept
   {
+    velocityStates.clear();
     contactConstraints.clear();
   }
 
@@ -112,14 +112,14 @@ namespace SHADE
   {
     const float INV_DT = 1.0f / dt;
 
-    for (auto& constraint : contactConstraints)
+    for (auto& [key, constraint] : contactConstraints)
     {
       const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB;
 
-      SHVec3 vA = velocityStates[constraint.idA].linearVelocity;
+      SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity;
-      SHVec3 wA = velocityStates[constraint.idA].angularVelocity;
+      SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity;
-      SHVec3 vB = velocityStates[constraint.idB].linearVelocity;
+      SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
-      SHVec3 wB = velocityStates[constraint.idB].angularVelocity;
+      SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity;
 
       for (uint32_t i = 0; i < constraint.numContacts; ++i)
       {
@@ -156,7 +156,7 @@ namespace SHADE
         contact.normalMass += SHVec3::Dot(RB_CROSS_N, constraint.invInertiaB * RB_CROSS_N);
 
         // Invert the normal mass (we want the actual mass, not the inverse mass)
-        contact.normalMass = contact.normalMass == 0.0f ? 0.0f : 1.0f / contact.normalMass;
+        contact.normalMass = 1.0f / contact.normalMass;
 
         // Effective mass along tangents (same steps as above)
 
@@ -169,7 +169,7 @@ namespace SHADE
           contact.tangentMass[j] += SHVec3::Dot(RA_CROSS_T, constraint.invInertiaA * RA_CROSS_T);
           contact.tangentMass[j] += SHVec3::Dot(RB_CROSS_T, constraint.invInertiaB * RB_CROSS_T);
 
-          contact.tangentMass[j] = contact.tangentMass[j] == 0.0f ? 0.0f : 1.0f / contact.tangentMass[j];
+          contact.tangentMass[j] = 1.0f / contact.tangentMass[j];
         }
 
         // Warm starting
@@ -195,28 +195,27 @@ namespace SHADE
         const SHVec3  RV_B = vB + SHVec3::Cross(wB, contact.rB);
         const float   RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal);
 
-        const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * contact.penetration;
+        const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * std::min(0.0f, -contact.penetration + PENETRATION_SLOP);
         const float RESTITUTION_BIAS = -constraint.restitution * RV_N;
 
         contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
       }
 
-      velocityStates[constraint.idA].linearVelocity   = vA;
+      velocityStates[key.GetEntityA()].linearVelocity   = vA;
-      velocityStates[constraint.idA].angularVelocity  = wA;
+      velocityStates[key.GetEntityA()].angularVelocity  = wA;
-      velocityStates[constraint.idB].linearVelocity   = vB;
+      velocityStates[key.GetEntityB()].linearVelocity   = vB;
-      velocityStates[constraint.idB].angularVelocity  = wB;
+      velocityStates[key.GetEntityB()].angularVelocity  = wB;
-
     }
   }
 
   void SHContactSolver::solve() noexcept
   {
-    for (auto& constraint : contactConstraints)
+    for (auto& [key, constraint] : contactConstraints)
     {
-      SHVec3 vA = velocityStates[constraint.idA].linearVelocity;
+      SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity;
-      SHVec3 wA = velocityStates[constraint.idA].angularVelocity;
+      SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity;
-      SHVec3 vB = velocityStates[constraint.idB].linearVelocity;
+      SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
-      SHVec3 wB = velocityStates[constraint.idB].angularVelocity;
+      SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity;
 
       for (uint32_t i = 0; i < constraint.numContacts; ++i)
       {
@@ -279,10 +278,10 @@ namespace SHADE
         wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE);
       }
 
-      velocityStates[constraint.idA].linearVelocity   = vA;
+      velocityStates[key.GetEntityA()].linearVelocity   = vA;
-      velocityStates[constraint.idA].angularVelocity  = wA;
+      velocityStates[key.GetEntityA()].angularVelocity  = wA;
-      velocityStates[constraint.idB].linearVelocity   = vB;
+      velocityStates[key.GetEntityB()].linearVelocity   = vB;
-      velocityStates[constraint.idB].angularVelocity  = wB;
+      velocityStates[key.GetEntityB()].angularVelocity  = wB;
     }
   }
 

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h

@@ -13,7 +13,6 @@
 
 // Project Headers
 #include "Constraints/SHContactConstraint.h"
-#include "SHContactManager.h"
 
 namespace SHADE
 {
@@ -41,7 +40,7 @@ namespace SHADE
     };
 
     using VelocityStates      = std::unordered_map<EntityID, VelocityState>; 
-    using ContactConstraints  = std::vector<SHContactConstraint>;
+    using ContactConstraints  = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
 
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */
@@ -66,14 +65,10 @@ namespace SHADE
      * Build a contact constraint from a new manifold.
      * @param manifold
      * A manifold to build a contact constraint from.
-     * @param rigidBodyA
-     * The rigid body belonging to the first collision shape.
-     * @param rigidBodyB
-     * The rigid body belonging to the second collision shape.
     */
-    void AddContact     (const SHManifold& manifold, const SHRigidBody* rigidBodyA, const SHRigidBody* rigidBodyB) noexcept;
+    void AddContact     (const SHCollisionKey& key, const SHManifold& manifold) noexcept;
 
-    void ClearContacts  () noexcept;
+    void Reset          () noexcept;
 
     /**
      * @brief

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -103,32 +103,11 @@ namespace SHADE
 
 
     /*
+     * TODO: A lot of this needs to be cleaned up.
      * Resolve Contacts
      */
 
-    // Build constraints
+    contactManager.SolveCollisions(settings.numVelocitySolverIterations, dt);
-    for (auto& [id, manifold] : contactManager.manifolds)
-    {
-      SHRigidBody* bodyA = rigidBodies[id.GetEntityA()];
-      SHRigidBody* bodyB = rigidBodies[id.GetEntityB()];
-
-      contactSolver.AddContact(manifold, bodyA, bodyB);
-    }
-
-    // Solve contacts
-    contactSolver.SolveContacts(settings.numVelocitySolverIterations, dt);
-
-    // Map velocities back to bodies
-    const auto& VELOCITY_STATES = contactSolver.GetVelocities();
-    for (auto& [id, velocityState] : VELOCITY_STATES)
-    {
-      SHRigidBody* body = rigidBodies[id];
-      body->linearVelocity  = velocityState.linearVelocity;
-      body->angularVelocity = velocityState.angularVelocity;
-    }
-
-    // Clear contacts
-    contactSolver.ClearContacts();
 
     /*
      * Integrate Velocities