From b4f13308de720524e91f07a03c82744d59b6eba5 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 13 Sep 2016 22:58:17 +0200
Subject: [PATCH] Optimize warmstarting in contact solver

---
 src/engine/ContactSolver.cpp | 141 +++++++++++++++++++++++++++++++++--
 src/engine/ContactSolver.h   |  24 ++++++
 src/engine/DynamicsWorld.cpp |   4 +-
 src/engine/Island.h          |   4 +-
 4 files changed, 164 insertions(+), 9 deletions(-)

diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 22359b10..4e2e0eec 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -38,15 +38,13 @@ const decimal ContactSolver::BETA = decimal(0.2);
 const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP= decimal(0.01);
 
-// TODO : Enable warmstarting again
-
 // Constructor
 ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
               :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
                mContactConstraints(nullptr), mPenetrationConstraints(nullptr),
                mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
                mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-               mIsWarmStartingActive(false), mIsSplitImpulseActive(true),
+               mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
                mIsSolveFrictionAtContactManifoldCenterActive(true) {
 
 }
@@ -83,7 +81,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
     assert(mFrictionConstraints != nullptr);
 
     // For each contact manifold of the island
-    ContactManifold** contactManifolds = island->getContactManifold();
+    ContactManifold** contactManifolds = island->getContactManifolds();
     for (uint i=0; i<mNbContactManifolds; i++) {
 
         ContactManifold* externalManifold = contactManifolds[i];
@@ -104,6 +102,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
 
         mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
         mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
+        mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold;
 
         // Get the position of the two bodies
         const Vector3& x1 = body1->mCenterOfMassWorld;
@@ -131,6 +130,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
         mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
         mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
         internalManifold.externalContactManifold = externalManifold;
+        mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
         //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
         //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
 
@@ -167,6 +167,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
             mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
             mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
             mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
+            mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
 
             // Get the contact point on the two bodies
             Vector3 p1 = externalContact->getWorldPointOnBody1();
@@ -178,7 +179,10 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
             //mPenetrationConstraints[penConstIndex].externalContact = externalContact;
             mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
             mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
-            //mPenetrationConstraints[penConstIndex].isRestingContact = externalContact->getIsRestingContact();
+            mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
+
+            mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
+
             externalContact->setIsRestingContact(true);
             //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
             //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
@@ -467,9 +471,116 @@ void ContactSolver::initializeContactConstraints() {
 /// the solution of the linear system
 void ContactSolver::warmStart() {
 
-    /*
     PROFILE("ContactSolver::warmStart()");
 
+    // Penetration constraints
+    for (uint i=0; i<mNbPenetrationConstraints; i++) {
+
+        // If it is not a new contact (this contact was already existing at last time step)
+        if (mPenetrationConstraints[i].isRestingContact) {
+
+            Vector3 linearImpulse = mPenetrationConstraints[i].normal * mPenetrationConstraints[i].penetrationImpulse;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 *
+                                                      (-linearImpulse);
+            mAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 *
+                                                       (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 *
+                                                      linearImpulse;
+            mAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 *
+                                                       (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+
+        }
+        else {  // If it is a new contact point
+
+            // Initialize the accumulated impulses to zero
+            mPenetrationConstraints[i].penetrationImpulse = 0.0;
+        }
+    }
+
+    // Friction constraints
+    for (uint i=0; i<mNbFrictionConstraints; i++) {
+
+        // If we solve the friction constraints at the center of the contact manifold and there is
+        // at least one resting contact point in the contact manifold
+        if (mFrictionConstraints[i].hasAtLeastOneRestingContactPoint) {
+
+            // Project the old friction impulses (with old friction vectors) into the new friction
+            // vectors to get the new friction impulses
+            Vector3 oldFrictionImpulse = mFrictionConstraints[i].friction1Impulse * mFrictionConstraints[i].oldFrictionVector1 +
+                                         mFrictionConstraints[i].friction2Impulse * mFrictionConstraints[i].oldFrictionVector2;
+            mFrictionConstraints[i].friction1Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector1);
+            mFrictionConstraints[i].friction2Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector2);
+
+            // ------ First friction constraint at the center of the contact manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            Vector3 linearImpulseBody2 = mFrictionConstraints[i].frictionVector1 * mFrictionConstraints[i].friction1Impulse;
+            Vector3 angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT1 * mFrictionConstraints[i].friction1Impulse;
+            Vector3 angularImpulseBody2 = mFrictionConstraints[i].r2CrossT1 * mFrictionConstraints[i].friction1Impulse;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
+            mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+            mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+            // ------ Second friction constraint at the center of the contact manifold ----- //
+
+            // Compute the impulse P = J^T * lambda
+            angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT2 * mFrictionConstraints[i].friction2Impulse;
+            linearImpulseBody2 = mFrictionConstraints[i].frictionVector2 * mFrictionConstraints[i].friction2Impulse;
+            angularImpulseBody2 = mFrictionConstraints[i].r2CrossT2 * mFrictionConstraints[i].friction2Impulse;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
+            mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+            mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+            // ------ Twist friction constraint at the center of the contact manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            angularImpulseBody2 = mFrictionConstraints[i].normal * mFrictionConstraints[i].frictionTwistImpulse;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+            // ------ Rolling resistance at the center of the contact manifold ------ //
+
+            // Compute the impulse P = J^T * lambda
+            angularImpulseBody2 = mFrictionConstraints[i].rollingResistanceImpulse;
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+            // Update the velocities of the body 1 by applying the impulse P
+            mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+        }
+        else {  // If it is a new contact manifold
+
+            // Initialize the accumulated impulses to zero
+            mFrictionConstraints[i].friction1Impulse = 0.0;
+            mFrictionConstraints[i].friction2Impulse = 0.0;
+            mFrictionConstraints[i].frictionTwistImpulse = 0.0;
+            mFrictionConstraints[i].rollingResistanceImpulse.setToZero();
+        }
+    }
+
+
+    /*
+
+
     // Check that warm starting is active
     if (!mIsWarmStartingActive) return;
 
@@ -1144,6 +1255,24 @@ void ContactSolver::solveFrictionConstraints() {
 // warm start the solver at the next iteration
 void ContactSolver::storeImpulses() {
 
+    // Penetration constraints
+    for (uint i=0; i<mNbPenetrationConstraints; i++) {
+
+        mPenetrationConstraints[i].contactPoint->setPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse);
+
+    }
+
+    // Friction constraints
+    for (uint i=0; i<mNbFrictionConstraints; i++) {
+
+        mFrictionConstraints[i].contactManifold->setFrictionImpulse1(mFrictionConstraints[i].friction1Impulse);
+        mFrictionConstraints[i].contactManifold->setFrictionImpulse2(mFrictionConstraints[i].friction2Impulse);
+        mFrictionConstraints[i].contactManifold->setFrictionTwistImpulse(mFrictionConstraints[i].frictionTwistImpulse);
+        mFrictionConstraints[i].contactManifold->setRollingResistanceImpulse(mFrictionConstraints[i].rollingResistanceImpulse);
+        mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
+        mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2);
+    }
+
     /*
     // For each contact manifold
     for (uint c=0; c<mNbContactManifolds; c++) {
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index ef0087a0..f3f5dfa7 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -114,6 +114,8 @@ class ContactSolver {
 
         struct PenetrationConstraint {
 
+            // TODO : Pack bools into a single value
+
             /// Index of body 1 in the constraint solver
             uint indexBody1;
 
@@ -167,10 +169,18 @@ class ContactSolver {
 
             /// Index of the corresponding friction constraint
             uint indexFrictionConstraint;
+
+            /// Pointer to the corresponding contact point
+            ContactPoint* contactPoint;
+
+            /// True if this constraint is for a resting contact
+            bool isRestingContact;
         };
 
         struct FrictionConstraint {
 
+            // TODO : Pack bools into a single value
+
             /// Index of body 1 in the constraint solver
             uint indexBody1;
 
@@ -260,6 +270,12 @@ class ContactSolver {
 
             /// Inverse inertia tensor of body 2
             Matrix3x3 inverseInertiaTensorBody2;
+
+            /// Pointer to the corresponding contact manifold
+            ContactManifold* contactManifold;
+
+            /// True if the original contact manifold has at least one resting contact
+            bool hasAtLeastOneRestingContactPoint;
         };
 
         // Structure ContactPointSolver
@@ -639,6 +655,9 @@ class ContactSolver {
 
         /// Clean up the constraint solver
         void cleanup();
+
+        /// Return true if warmstarting is active
+        bool IsWarmStartingActive() const;
 };
 
 // Set the split velocities arrays
@@ -731,6 +750,11 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
                    contactPoint.r2CrossT2 * deltaLambda);
 }
 
+// Return true if warmstarting is active
+inline bool ContactSolver::IsWarmStartingActive() const {
+    return mIsWarmStartingActive;
+}
+
 }
 
 #endif
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 4bd7aa19..189ef0e0 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -381,7 +381,9 @@ void DynamicsWorld::solveContactsAndConstraints() {
             mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
 
             // Warm start the contact solver
-            mContactSolver.warmStart();
+            if (mContactSolver.IsWarmStartingActive()) {
+                mContactSolver.warmStart();
+            }
         }
 
         // If there are constraints
diff --git a/src/engine/Island.h b/src/engine/Island.h
index 1b086939..512a0d1b 100644
--- a/src/engine/Island.h
+++ b/src/engine/Island.h
@@ -114,7 +114,7 @@ class Island {
         RigidBody** getBodies();
 
         /// Return a pointer to the array of contact manifolds
-        ContactManifold** getContactManifold();
+        ContactManifold** getContactManifolds();
 
         /// Return a pointer to the array of joints
         Joint** getJoints();
@@ -164,7 +164,7 @@ inline RigidBody** Island::getBodies() {
 }
 
 // Return a pointer to the array of contact manifolds
-inline ContactManifold** Island::getContactManifold() {
+inline ContactManifold** Island::getContactManifolds() {
     return mContactManifolds;
 }