Implement the split impulse technique for position correction

src/engine/ConstraintSolver.cpp

@@ -31,11 +31,11 @@
 using namespace reactphysics3d;
 using namespace std;
 
-
 // Constructor
 ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
     :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0),
-      mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(false) {
+      mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true),
+      mIsSplitImpulseActive(true) {
 
 }
 
@@ -118,6 +118,8 @@ void ConstraintSolver::initialize() {
 
     mLinearVelocities = new Vector3[nbBodies];
     mAngularVelocities = new Vector3[nbBodies];
+    mSplitLinearVelocities = new Vector3[nbBodies];
+    mSplitAngularVelocities = new Vector3[nbBodies];
 
     assert(mMapBodyToIndex.size() == nbBodies);
 }
@@ -136,6 +138,8 @@ void ConstraintSolver::initializeBodies() {
 
         mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
         mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
+        mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
+        mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
     }
 }
 
@@ -204,7 +208,7 @@ void ConstraintSolver::initializeContactConstraints() {
             contact.restitutionBias = 0.0;
             decimal deltaVDotN = deltaV.dot(contact.normal);
             // TODO : Use a constant here
-            if (!contact.isRestingContact) {
+            if (deltaVDotN < 1.0f) {
                 contact.restitutionBias = constraint.restitutionFactor * deltaVDotN;
             }
 
@@ -212,6 +216,9 @@ void ConstraintSolver::initializeContactConstraints() {
             contact.penetrationImpulse = realContact->getCachedLambda(0);
             contact.friction1Impulse = realContact->getCachedLambda(1);
             contact.friction2Impulse = realContact->getCachedLambda(2);
+
+            // Initialize the split impulses to zero
+            contact.penetrationSplitImpulse = 0.0;
         }
     }
 }
@@ -323,17 +330,19 @@ void ConstraintSolver::solveContactConstraints() {
                 // Compute the bias "b" of the constraint
                 decimal beta = 0.2;
                 // TODO : Use a constant for the slop
-                decimal slop = 0.005;
+                decimal slop = 0.01;
                 decimal biasPenetrationDepth = 0.0;
                 if (contact.penetrationDepth > slop) biasPenetrationDepth = -(beta/mTimeStep) *
                         max(0.0f, float(contact.penetrationDepth - slop));
                 decimal b = biasPenetrationDepth + contact.restitutionBias;
 
                 // Compute the Lagrange multiplier
-                deltaLambda = - (Jv + b);
+                if (mIsSplitImpulseActive) {
-
+                    deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass;
-                //deltaLambda -= contact.b_Penetration;
+                }
-                deltaLambda *= contact.inversePenetrationMass;
+                else {
+                    deltaLambda = - (Jv + b) * contact.inversePenetrationMass;
+                }
                 lambdaTemp = contact.penetrationImpulse;
                 contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f);
                 deltaLambda = contact.penetrationImpulse - lambdaTemp;
@@ -349,6 +358,32 @@ void ConstraintSolver::solveContactConstraints() {
                 // Apply the impulse to the bodies of the constraint
                 applyImpulse(impulsePenetration, constraint);
 
+                // If the split impulse position correction is active
+                if (mIsSplitImpulseActive) {
+
+                    // Split impulse (position correction)
+                    const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1];
+                    const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1];
+                    const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2];
+                    const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2];
+                    Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1);
+                    decimal JvSplit = deltaVSplit.dot(contact.normal);
+                    decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass;
+                    decimal lambdaTempSplit = contact.penetrationSplitImpulse;
+                    contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
+                    deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit;
+
+                    // Compute the impulse P=J^T * lambda
+                    linearImpulseBody1 = -contact.normal * deltaLambdaSplit;
+                    angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit;
+                    linearImpulseBody2 = contact.normal * deltaLambdaSplit;
+                    angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit;
+                    const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1,
+                                                          linearImpulseBody2, angularImpulseBody2);
+
+                    applySplitImpulse(splitImpulsePenetration, constraint);
+                }
+
                 // --------- Friction 1 --------- //
 
                 // Compute J*v
@@ -467,6 +502,24 @@ void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstra
     }
 }
 
+// Apply an impulse to the two bodies of a constraint
+void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) {
+
+    // Update the velocities of the bodies by applying the impulse P
+    if (constraint.isBody1Moving) {
+        mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 *
+                                                    impulse.linearImpulseBody1;
+        mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 *
+                                                     impulse.angularImpulseBody1;
+    }
+    if (constraint.isBody2Moving) {
+        mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 *
+                                                    impulse.linearImpulseBody2;
+        mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 *
+                                                     impulse.angularImpulseBody2;
+    }
+}
+
 // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
 // The two vectors have to be such that : t1 x t2 = contactNormal
 void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity,

src/engine/ConstraintSolver.h

@@ -64,6 +64,7 @@ struct ContactPointConstraint {
     decimal penetrationImpulse;             // Accumulated normal impulse
     decimal friction1Impulse;               // Accumulated impulse in the 1st friction direction
     decimal friction2Impulse;               // Accumulated impulse in the 2nd friction direction
+    decimal penetrationSplitImpulse;        // Accumulated split impulse for penetration correction
     Vector3 normal;                         // Normal vector of the contact
     Vector3 frictionVector1;                // First friction vector in the tangent plane
     Vector3 frictionVector2;                // Second friction vector in the tangent plane
@@ -146,6 +147,13 @@ class ConstraintSolver {
                                                         // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
         Vector3* mLinearVelocities;                     // Array of constrained linear velocities
         Vector3* mAngularVelocities;                    // Array of constrained angular velocities
+
+        // Split linear velocities for the position contact solver (split impulse)
+        Vector3* mSplitLinearVelocities;
+
+        // Split angular velocities for the position contact solver (split impulse)
+        Vector3* mSplitAngularVelocities;
+
         decimal mTimeStep;                              // Current time step
 
         // Contact constraints
@@ -163,6 +171,9 @@ class ConstraintSolver {
         // True if the warm starting of the solver is active
         bool mIsWarmStartingActive;
 
+        // True if the split impulse position correction is active
+        bool mIsSplitImpulseActive;
+
         // -------------------- Methods -------------------- //
 
         // Initialize the constraint solver
@@ -187,6 +198,9 @@ class ConstraintSolver {
         // Apply an impulse to the two bodies of a constraint
         void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint);
 
+        // Apply an impulse to the two bodies of a constraint
+        void applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint);
+
         // Compute the collision restitution factor from the restitution factor of each body
         decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const;
 
@@ -214,9 +228,15 @@ class ConstraintSolver {
         // Return the constrained linear velocity of a body after solving the constraints
         Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
 
+        // Return the split linear velocity
+        Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
+
         // Return the constrained angular velocity of a body after solving the constraints
         Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
 
+        // Return the split angular velocity
+        Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
+
         // Clean up the constraint solver
         void cleanup();
 
@@ -232,15 +252,29 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
 // Return the constrained linear velocity of a body after solving the constraints
 inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
     assert(isConstrainedBody(body));
-    uint indexBodyArray = mMapBodyToIndex[body];
+    uint indexBody = mMapBodyToIndex[body];
-    return mLinearVelocities[indexBodyArray];
+    return mLinearVelocities[indexBody];
+}
+
+// Return the split linear velocity
+inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
+    assert(isConstrainedBody(body));
+    uint indexBody = mMapBodyToIndex[body];
+    return mSplitLinearVelocities[indexBody];
 }
 
 // Return the constrained angular velocity of a body after solving the constraints
 inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
     assert(isConstrainedBody(body));
-    uint indexBodyArray = mMapBodyToIndex[body];
+    uint indexBody = mMapBodyToIndex[body];
-    return mAngularVelocities[indexBodyArray];
+    return mAngularVelocities[indexBody];
+}
+
+// Return the split angular velocity
+inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
+    assert(isConstrainedBody(body));
+    uint indexBody = mMapBodyToIndex[body];
+    return mSplitAngularVelocities[indexBody];
 }
 
 // Clean up the constraint solver

src/engine/DynamicsWorld.cpp

@@ -120,8 +120,8 @@ void DynamicsWorld::updateAllBodiesMotion() {
             // If it's a constrained body
             if (mConstraintSolver.isConstrainedBody(*it)) {
                 // Get the constrained linear and angular velocities from the constraint solver
-                newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
+                newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody);
-                newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
+                newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody);
             }
             else {
                 // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
@@ -147,8 +147,8 @@ void DynamicsWorld::updateAllBodiesMotion() {
 // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
 // orientation of the body
 void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
-                                                       const Vector3& newLinVelocity,
+                                                       Vector3 newLinVelocity,
-                                                       const Vector3& newAngVelocity) {
+                                                       Vector3 newAngVelocity) {
     decimal dt = mTimer.getTimeStep();
 
     assert(rigidBody);
@@ -160,6 +160,12 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
     rigidBody->setLinearVelocity(newLinVelocity);
     rigidBody->setAngularVelocity(newAngVelocity);
 
+    // Split velocity (only used to update the position)
+    if (mConstraintSolver.isConstrainedBody(rigidBody)) {
+        newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody);
+        newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody);
+    }
+    
     // Get current position and orientation of the body
     const Vector3& currentPosition = rigidBody->getTransform().getPosition();
     const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();

src/engine/DynamicsWorld.h

@@ -96,8 +96,8 @@ class DynamicsWorld : public CollisionWorld {
         void updateAllBodiesMotion();
 
         // Update the position and orientation of a body
-        void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
+        void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
-                                                const Vector3& newAngVelocity);
+                                                Vector3 newAngVelocity);
 
         // Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();