Continue to implement the Sequential Impulse solver

src/engine/ConstraintSolver.cpp

@@ -86,6 +86,7 @@ void ConstraintSolver::initialize() {
         constraint.massInverseBody1 = body1->getMassInverse();
         constraint.massInverseBody2 = body2->getMassInverse();
         constraint.nbContacts = contactManifold.nbContacts;
+        constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2);
 
         // For each contact point of the contact manifold
         for (uint c=0; c<contactManifold.nbContacts; c++) {
@@ -104,6 +105,7 @@ void ConstraintSolver::initialize() {
             contactPointConstraint.r2 = p2 - x2;
             contactPointConstraint.frictionVector1 = contact->getFrictionVector1();
             contactPointConstraint.frictionVector2 = contact->getFrictionVector2();
+            contactPointConstraint.penetrationDepth = contact->getPenetrationDepth();
         }
 
         mNbContactConstraints++;
@@ -200,6 +202,24 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
                 contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2);
             }
 
+            const Vector3& v1 = mLinearVelocities[constraint.indexBody1];
+            const Vector3& w1 = mAngularVelocities[constraint.indexBody1];
+            const Vector3& v2 = mLinearVelocities[constraint.indexBody2];
+            const Vector3& w2 = mAngularVelocities[constraint.indexBody2];
+
+            // Compute the restitution velocity bias "b". We compute this here instead
+            // of inside the solve() method because we need to use the velocity difference
+            // at the beginning of the contact. Note that if it is a resting contact (normal velocity
+            // under a given threshold), we don't add a restitution velocity bias
+            contact.restitutionBias = 0.0;
+            Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1);
+            decimal deltaVDotN = deltaV.dot(contact.normal);
+            // TODO : Use a constant here
+            decimal elasticLinearVelocityThreshold = 0.3;
+            if (deltaVDotN < -elasticLinearVelocityThreshold) {
+                contact.restitutionBias = constraint.restitutionFactor * deltaVDotN;
+            }
+
             // Fill in the J_sp matrix
             realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration);
             realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1);
@@ -382,14 +402,38 @@ void ConstraintSolver::solveLCP() {
                 indexBody1Array = constraint.indexBody1;
                 indexBody2Array = constraint.indexBody2;
 
+                const Vector3& v1 = mLinearVelocities[constraint.indexBody1];
+                const Vector3& w1 = mAngularVelocities[constraint.indexBody1];
+                const Vector3& v2 = mLinearVelocities[constraint.indexBody2];
+                const Vector3& w2 = mAngularVelocities[constraint.indexBody2];
+
                 // --------- Penetration --------- //
 
+                // Compute J*v
+                Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1);
+                decimal deltaVDotN = deltaV.dot(contact.normal);
+                decimal Jv = deltaVDotN;
+
+                // Compute the bias "b" of the constraint
+                decimal beta = 0.2;
+                // TODO : Use a constant for the slop
+                decimal slop = 0.005;
+                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);
+
+                /*
                 deltaLambda = 0.0;
                 for (uint j=0; j<3; j++) {
                     deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]);
                     deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]);
                 }
-                deltaLambda -= contact.b_Penetration;
+                */
+                //deltaLambda -= contact.b_Penetration;
                 deltaLambda /= contact.inversePenetrationMass;
                 lambdaTemp = contact.penetrationImpulse;
                 contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
@@ -537,3 +581,23 @@ void ConstraintSolver::solve(decimal dt) {
     // Cache the lambda values in order to use them in the next step
     cacheLambda();
 }
+
+// Apply an impulse to the two bodies of a constraint
+void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) {
+
+    // Update the velocities of the bodies by applying the impulse P
+    if (constraint.isBody1Moving) {
+        mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 *
+                                                    impulse.linearImpulseBody1;
+        mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 *
+                                                     impulse.angularImpulseBody1;
+    }
+    if (constraint.isBody2Moving) {
+        mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 *
+                                                    impulse.linearImpulseBody2;
+        mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 *
+                                                     impulse.angularImpulseBody2;
+    }
+}
+
+

src/engine/ConstraintSolver.h

@@ -39,6 +39,24 @@ namespace reactphysics3d {
 // Declarations
 class DynamicsWorld;
 
+// Structure Impulse
+struct Impulse {
+
+    public:
+        Vector3& linearImpulseBody1;
+        Vector3& linearImpulseBody2;
+        Vector3& angularImpulseBody1;
+        Vector3& angularImpulseBody2;
+
+        // Constructor
+        Impulse(Vector3& linearImpulseBody1, Vector3& angularImpulseBody1,
+                Vector3& linearImpulseBody2, Vector3& angularImpulseBody2)
+            : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
+              linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
+
+        }
+};
+
 // Structure ContactPointConstraint
 // Internal structure for a contact point constraint
 struct ContactPointConstraint {
@@ -137,9 +155,7 @@ class ConstraintSolver {
         std::vector<Constraint*> activeConstraints;     // Current active constraints in the physics world
         bool isErrorCorrectionActive;                   // True if error correction (with world order) is active
         uint mNbIterations;                           // Number of iterations of the LCP solver
-        uint nbIterationsLCPErrorCorrection;            // Number of iterations of the LCP solver for error correction
         uint nbConstraints;                             // Total number of constraints (with the auxiliary constraints)
-        uint nbConstraintsError;                        // Number of constraints for error correction projection (only contact constraints)
         uint nbBodies;                                  // Current number of bodies in the physics world
         RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2];       // 2-dimensional array that contains the mapping of body reference
                                                         // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
@@ -151,21 +167,8 @@ class ConstraintSolver {
         decimal B_sp[2][6*NB_MAX_CONSTRAINTS];          // 2-dimensional array that correspond to a useful matrix in sparse representation
                                                         // This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
                                                         // a 6x1 matrix
-        decimal J_spError[NB_MAX_CONSTRAINTS][2*6];     // Same as J_sp for error correction projection (only contact constraints)
-        decimal B_spError[2][6*NB_MAX_CONSTRAINTS];     // Same as B_sp for error correction projection (only contact constraints)
         decimal b[NB_MAX_CONSTRAINTS];                  // Vector "b" of the LCP problem
-        decimal bError[NB_MAX_CONSTRAINTS];             // Vector "b" of the LCP problem for error correction projection
         decimal d[NB_MAX_CONSTRAINTS];                  // Vector "d"
-        decimal aError[6*NB_MAX_BODIES];                // Vector "a" for error correction projection
-        decimal penetrationDepths[NB_MAX_CONSTRAINTS];  // Array of penetration depths for error correction projection
-        decimal lambda[NB_MAX_CONSTRAINTS];             // Lambda vector of the LCP problem
-        decimal lambdaError[NB_MAX_CONSTRAINTS];        // Lambda vector of the LCP problem for error correction projections
-        decimal lambdaInit[NB_MAX_CONSTRAINTS];         // Lambda init vector for the LCP solver
-        decimal errorValues[NB_MAX_CONSTRAINTS];        // Error vector of all constraints
-        decimal lowerBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the low limits for the variables of the LCP problem
-        decimal upperBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the high limits for the variables of the LCP problem
-        decimal lowerBoundsError[NB_MAX_CONSTRAINTS];   // Same as "lowerBounds" but for error correction projections
-        decimal upperBoundsError[NB_MAX_CONSTRAINTS];   // Same as "upperBounds" but for error correction projections
         Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES];       // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
         decimal Minv_sp_mass_diag[NB_MAX_BODIES];       // Array that contains for each body the inverse of its mass
                                                         // This is an array of size nbBodies that contains in each cell a 6x6 matrix
@@ -194,6 +197,12 @@ class ConstraintSolver {
         void warmStart();                          // Compute the vector a used in the solve() method
         void solveLCP();                                // Solve a LCP problem using Projected-Gauss-Seidel algorithm
 
+        // Apply an impulse to the two bodies of a constraint
+        void applyImpulse(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;
+
    public:
         ConstraintSolver(DynamicsWorld* world);                         // Constructor
         virtual ~ConstraintSolver();                                   // Destructor
@@ -249,6 +258,16 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
     mNbIterations = nbIterations;
 }
 
+// Compute the collision restitution factor from the restitution factor of each body
+inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1,
+                                                             const RigidBody* body2) const {
+    decimal restitution1 = body1->getRestitution();
+    decimal restitution2 = body2->getRestitution();
+
+    // Return the largest restitution factor
+    return (restitution1 > restitution2) ? restitution1 : restitution2;
+}
+
 } // End of ReactPhysics3D namespace
 
 #endif