Implemented axis locking constraints

Assets/Scenes/PhysicsSandbox.shade

@@ -4,7 +4,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.186280191, y: 4.3224473, z: 0}
+      Translate: {x: 0, y: 3, z: 0}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -19,6 +19,107 @@
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: true
+      Colliders:
+        - Is Trigger: false
+          Type: Sphere
+          Radius: 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:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500
+- EID: 1
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Camera Component:
+      Position: {x: 0, y: 0.5, z: 5}
+      Pitch: 0
+      Yaw: 0
+      Roll: 0
+      Width: 1920
+      Height: 1080
+      Near: 0.00999999978
+      Far: 10000
+      Perspective: true
+      IsActive: true
+  Scripts: ~
+- EID: 2
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: -2.5, z: 0}
+      Rotate: {x: 0, y: 0, z: 0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: false
+      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:
+      DrawColliders: true
+      Colliders:
+        - Is Trigger: false
+          Type: Sphere
+          Radius: 5
+          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: 3
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 5, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      Drag: 1
+      Angular Drag: 1
+      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
@@ -42,60 +143,3 @@
       Enabled: true
       forceAmount: 50
       torqueAmount: 500
-- EID: 1
-  Name: Default
-  IsActive: true
-  NumberOfChildren: 0
-  Components:
-    Camera Component:
-      Position: {x: 0, y: 0.5, z: 3}
-      Pitch: 0
-      Yaw: 0
-      Roll: 0
-      Width: 1920
-      Height: 1080
-      Near: 0.00999999978
-      Far: 10000
-      Perspective: true
-      IsActive: true
-  Scripts: ~
-- EID: 2
-  Name: Default
-  IsActive: true
-  NumberOfChildren: 0
-  Components:
-    Transform Component:
-      Translate: {x: 0, y: 1, z: 0}
-      Rotate: {x: 0, y: 0, z: 0}
-      Scale: {x: 1, y: 1, z: 1}
-      IsActive: true
-    RigidBody Component:
-      Type: Static
-      Auto Mass: false
-      Mass: .inf
-      Drag: 0.00999999978
-      Angular Drag: 0.00999999978
-      Use Gravity: false
-      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:
-      DrawColliders: true
-      Colliders:
-        - Is Trigger: false
-          Type: Sphere
-          Radius: 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: ~

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

@@ -0,0 +1,56 @@
+/****************************************************************************************
+ * \file      SHVelocityState.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for a Velocity State for constraint solving.
+ *
+ * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
+ *            disclosure of this file or its contents without the prior written consent
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#pragma once
+
+// Project Headers
+#include "Physics/Dynamics/SHRigidBody.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Type Definitions                                                                  */
+  /*-----------------------------------------------------------------------------------*/
+
+  struct SH_API SHVelocityState
+  {
+  public:
+    SHVec3 LinearVelocity;
+    SHVec3 AngularVelocity;
+
+    SHVec3 LinearLockFactor;
+    SHVec3 AngularLockFactor;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHVelocityState (const SHRigidBody* rigidBody) noexcept
+    {
+      LinearVelocity    = rigidBody->GetLinearVelocity();
+      AngularVelocity   = rigidBody->GetAngularVelocity();
+
+      LinearLockFactor  = SHVec3
+      {
+        rigidBody->GetFreezePositionX() ? 0.0f :  1.0f
+      , rigidBody->GetFreezePositionY() ? 0.0f :  1.0f
+      , rigidBody->GetFreezePositionZ() ? 0.0f :  1.0f
+      };
+
+      AngularLockFactor = SHVec3
+      {
+        rigidBody->GetFreezeRotationX() ? 0.0f :  1.0f
+      , rigidBody->GetFreezeRotationY() ? 0.0f :  1.0f
+      , rigidBody->GetFreezeRotationZ() ? 0.0f :  1.0f
+      };
+    }
+  };
+
+} // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -193,11 +193,11 @@ namespace SHADE
       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);
+      bodyA->SetLinearVelocity(STATE_A.LinearVelocity);
-      bodyB->SetLinearVelocity(STATE_B.linearVelocity);
+      bodyB->SetLinearVelocity(STATE_B.LinearVelocity);
 
-      bodyA->SetAngularVelocity(STATE_A.angularVelocity);
+      bodyA->SetAngularVelocity(STATE_A.AngularVelocity);
-      bodyB->SetAngularVelocity(STATE_B.angularVelocity);
+      bodyB->SetAngularVelocity(STATE_B.AngularVelocity);
     }
 
     contactSolver.Reset();

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -47,8 +47,8 @@ namespace SHADE
     const auto* BODY_B  = manifold.bodyB;
 
     // Add velocities if it doesn't already exist
-    velocityStates.emplace(key.GetEntityA(), VelocityState{ BODY_A->linearVelocity, BODY_A->angularVelocity });
+    velocityStates.emplace(key.GetEntityA(), SHVelocityState{ BODY_A });
-    velocityStates.emplace(key.GetEntityB(), VelocityState{ BODY_B->linearVelocity, BODY_B->angularVelocity });
+    velocityStates.emplace(key.GetEntityB(), SHVelocityState{ BODY_B });
 
     // Mix friction & restitution
     const float FRICTION_A    = SHAPE_A->GetFriction();
@@ -116,10 +116,18 @@ namespace SHADE
     {
       const float INV_MASS_SUM = constraint.invMassA + constraint.invMassB;
 
-      SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity;
+      SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
-      SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity;
+      SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
-      SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
+
-      SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity;
+      SHVec3 vA = velocityStateA.LinearVelocity;
+      SHVec3 wA = velocityStateA.AngularVelocity;
+      SHVec3 vB = velocityStateB.LinearVelocity;
+      SHVec3 wB = velocityStateB.AngularVelocity;
+
+      const SHVec3& LINEAR_LOCK_A   = velocityStateA.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_A  = velocityStateA.AngularLockFactor;
+      const SHVec3& LINEAR_LOCK_B   = velocityStateB.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_B  = velocityStateB.AngularLockFactor;
 
       for (uint32_t i = 0; i < constraint.numContacts; ++i)
       {
@@ -172,19 +180,6 @@ namespace SHADE
           contact.tangentMass[j] = 1.0f / contact.tangentMass[j];
         }
 
-        // Warm starting
-        // Compute impulses
-        SHVec3 impulse = constraint.normal * contact.normalImpulse;
-        for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
-          impulse += constraint.tangents[j] * contact.tangentImpulse[j];
-
-        // Apply impulses onto velocities
-        vA -= impulse * constraint.invMassA;
-        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, impulse);
-
-        vB += impulse * constraint.invMassB;
-        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse);
-
         // Calculate bias per contact
         /*
          * error bias       = baumgarte factor / dt * penetration
@@ -199,12 +194,25 @@ namespace SHADE
         const float RESTITUTION_BIAS = -constraint.restitution * RV_N;
 
         contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
+
+        // Warm starting
+        // Compute impulses
+        SHVec3 impulse = constraint.normal * contact.normalImpulse;
+        for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
+          impulse += constraint.tangents[j] * contact.tangentImpulse[j];
+
+        // Apply impulses onto velocities
+        vA -= impulse * constraint.invMassA * LINEAR_LOCK_A;
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, impulse) * ANGULAR_LOCK_A;
+
+        vB += impulse * constraint.invMassB * LINEAR_LOCK_B;
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B;
       }
 
-      velocityStates[key.GetEntityA()].linearVelocity   = vA;
+      velocityStateA.LinearVelocity   = vA;
-      velocityStates[key.GetEntityA()].angularVelocity  = wA;
+      velocityStateA.AngularVelocity  = wA;
-      velocityStates[key.GetEntityB()].linearVelocity   = vB;
+      velocityStateB.LinearVelocity   = vB;
-      velocityStates[key.GetEntityB()].angularVelocity  = wB;
+      velocityStateB.AngularVelocity  = wB;
     }
   }
 
@@ -212,10 +220,18 @@ namespace SHADE
   {
     for (auto& [key, constraint] : contactConstraints)
     {
-      SHVec3 vA = velocityStates[key.GetEntityA()].linearVelocity;
+      SHVelocityState& velocityStateA = velocityStates.find(key.GetEntityA())->second;
-      SHVec3 wA = velocityStates[key.GetEntityA()].angularVelocity;
+      SHVelocityState& velocityStateB = velocityStates.find(key.GetEntityB())->second;
-      SHVec3 vB = velocityStates[key.GetEntityB()].linearVelocity;
+
-      SHVec3 wB = velocityStates[key.GetEntityB()].angularVelocity;
+      SHVec3 vA = velocityStateA.LinearVelocity;
+      SHVec3 wA = velocityStateA.AngularVelocity;
+      SHVec3 vB = velocityStateB.LinearVelocity;
+      SHVec3 wB = velocityStateB.AngularVelocity;
+
+      const SHVec3& LINEAR_LOCK_A   = velocityStateA.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_A  = velocityStateA.AngularLockFactor;
+      const SHVec3& LINEAR_LOCK_B   = velocityStateB.LinearLockFactor;
+      const SHVec3& ANGULAR_LOCK_B  = velocityStateB.AngularLockFactor;
 
       for (uint32_t i = 0; i < constraint.numContacts; ++i)
       {
@@ -245,11 +261,11 @@ namespace SHADE
           const SHVec3 TANGENT_IMPULSE = newTangentImpulse * constraint.tangents[j];
 
           // Apply impulses
-          vA -= TANGENT_IMPULSE * constraint.invMassA;
+          vA -= TANGENT_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
-          wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE);
+          wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, TANGENT_IMPULSE) * ANGULAR_LOCK_A;
 
-          vB += TANGENT_IMPULSE * constraint.invMassB;
+          vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
-          wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE);
+          wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
         }
 
         // Solve normal impulse
@@ -271,17 +287,17 @@ namespace SHADE
         const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
 
         // Apply impulses
-        vA -= NORMAL_IMPULSE * constraint.invMassA;
+        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
-        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE);
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
 
-        vB += NORMAL_IMPULSE * constraint.invMassB;
+        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
-        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE);
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
       }
 
-      velocityStates[key.GetEntityA()].linearVelocity   = vA;
+      velocityStateA.LinearVelocity   = vA;
-      velocityStates[key.GetEntityA()].angularVelocity  = wA;
+      velocityStateA.AngularVelocity  = wA;
-      velocityStates[key.GetEntityB()].linearVelocity   = vB;
+      velocityStateB.LinearVelocity   = vB;
-      velocityStates[key.GetEntityB()].angularVelocity  = wB;
+      velocityStateB.AngularVelocity  = wB;
     }
   }
 

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h

@@ -13,6 +13,7 @@
 
 // Project Headers
 #include "Constraints/SHContactConstraint.h"
+#include "Constraints/SHVelocityState.h"
 
 namespace SHADE
 {
@@ -31,15 +32,7 @@ namespace SHADE
     /* Type Definitions                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    struct VelocityState
+    using VelocityStates      = std::unordered_map<EntityID, SHVelocityState>; 
-    {
-      // Velocities
-
-      SHVec3 linearVelocity;
-      SHVec3 angularVelocity;
-    };
-
-    using VelocityStates      = std::unordered_map<EntityID, VelocityState>; 
     using ContactConstraints  = std::unordered_map<SHCollisionKey, SHContactConstraint, SHCollisionKeyHash>;
 
     /*---------------------------------------------------------------------------------*/

SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp

@@ -143,29 +143,13 @@ namespace SHADE
     // Apply drag (exponentially applied)
     rigidBody.linearVelocity  *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
     rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag);
+
+    rigidBody.constrainLinearVelocities();
+    rigidBody.constrainAngularVelocities();
   }
 
   void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
   {
-    static const auto ENFORCE_CONSTRAINED_VELOCITIES = [](SHRigidBody& rigidBody)
-    {
-      // Enforce linear constraints
-      rigidBody.linearVelocity = SHVec3
-      {
-        rigidBody.GetFreezePositionX() ? 0.0f : rigidBody.linearVelocity.x
-      , rigidBody.GetFreezePositionY() ? 0.0f : rigidBody.linearVelocity.y
-      , rigidBody.GetFreezePositionZ() ? 0.0f : rigidBody.linearVelocity.z
-      };
-
-      // Enforce angular constraints
-      rigidBody.angularVelocity = SHVec3
-      {
-        rigidBody.GetFreezeRotationX() ? 0.0f : rigidBody.angularVelocity.x
-      , rigidBody.GetFreezeRotationY() ? 0.0f : rigidBody.angularVelocity.y
-      , rigidBody.GetFreezeRotationZ() ? 0.0f : rigidBody.angularVelocity.z
-      };
-    };
-
     // Always reset movement flag
     rigidBody.motionState.hasMoved = false;
 
@@ -178,7 +162,8 @@ namespace SHADE
     // Both dynamic and kinematic can sleep when their velocities are under the thresholds.
     else if (!rigidBody.IsSleeping())
     {
-      ENFORCE_CONSTRAINED_VELOCITIES(rigidBody);
+      rigidBody.constrainLinearVelocities();
+      rigidBody.constrainAngularVelocities();
 
       rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
       rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt);

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -200,6 +200,8 @@ namespace SHADE
 
   const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
   {
+    // Check if linear velocity needs to be constrained
+
     return linearVelocity;
   }
 
@@ -378,6 +380,7 @@ namespace SHADE
     }
 
     linearVelocity = newLinearVelocity;
+    constrainLinearVelocities();
   }
 
   void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
@@ -389,6 +392,7 @@ namespace SHADE
     }
 
     angularVelocity = newAngularVelocity;
+    constrainAngularVelocities();
   }
 
   void SHRigidBody::SetIsActive(bool isActive) noexcept
@@ -676,7 +680,20 @@ namespace SHADE
       const auto* FIRST_SHAPE = collider->GetCollisionShape(0);
       localInvInertia = SHMatrix::Inverse(FIRST_SHAPE->GetInertiaTensor(1.0f / invMass));
     }
+  }
 
+  void SHRigidBody::constrainLinearVelocities() noexcept
+  {
+    linearVelocity.x = GetFreezePositionX() ? 0.0f : linearVelocity.x;
+    linearVelocity.y = GetFreezePositionY() ? 0.0f : linearVelocity.y;
+    linearVelocity.z = GetFreezePositionZ() ? 0.0f : linearVelocity.z;
+  }
+
+  void SHRigidBody::constrainAngularVelocities() noexcept
+  {
+    angularVelocity.x = GetFreezeRotationX() ? 0.0f : angularVelocity.x;
+    angularVelocity.y = GetFreezeRotationY() ? 0.0f : angularVelocity.y;
+    angularVelocity.z = GetFreezeRotationZ() ? 0.0f : angularVelocity.z;
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h

@@ -249,7 +249,7 @@ namespace SHADE
     SHVec3        linearVelocity;
     SHVec3        angularVelocity;
 
-    // aZ aY aX pZ pY pX 0 0 0 0 inIsland autoMass enableGravity enableSleeping sleeping active
+    // aZ aY aX rotLockActive pZ pY pX posLockActive 0 0 inIsland autoMass enableGravity enableSleeping sleeping active
     uint16_t      flags;
 
     SHMotionState motionState;
@@ -263,6 +263,9 @@ namespace SHADE
     void computeInertiaTensor       ()  noexcept;
     void computeMassAndInertiaTensor()  noexcept;
 
+    void constrainLinearVelocities  ()  noexcept;
+    void constrainAngularVelocities ()  noexcept;
+    
   };
 
 } // namespace SHADE