Add contact caching to improve the convergence rate of the constraint solver

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@365 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/engine/ConstraintSolver.cpp

@@ -83,6 +83,7 @@ void ConstraintSolver::allocate() {
     errorValues.changeSize(nbConstraints);
     b.changeSize(nbConstraints);
     lambda.changeSize(nbConstraints);
+    lambdaInit.changeSize(nbConstraints);
     lowerBounds.changeSize(nbConstraints);
     upperBounds.changeSize(nbConstraints);
     Minv_sp = new Matrix[nbBodies];
@@ -119,6 +120,25 @@ void ConstraintSolver::fillInMatrices() {
         // Fill in the error vector
         errorValues.setValue(noConstraint, constraint->getErrorValue());
 
+        // If it's a contact constraint
+        Contact* contact = dynamic_cast<Contact*>(constraint);
+        if (contact) {
+            // Get the lambda init value from the cache if exists
+            ContactCachingInfo* contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint());
+            if (contactInfo) {
+                // The last lambda init value was in the cache
+                lambdaInit.setValue(noConstraint, contactInfo->lambda);
+            }
+            else {
+                // The las lambda init value was not in the cache
+                lambdaInit.setValue(noConstraint, 0.0);
+            }
+        }
+        else {
+            // Set the lambda init value
+            lambdaInit.setValue(noConstraint, 0.0);
+        }
+
         nbAuxConstraints = constraint->getNbAuxConstraints();
 
         // If the current constraint has auxiliary constraints
@@ -135,6 +155,9 @@ void ConstraintSolver::fillInMatrices() {
                 // Fill in the body mapping matrix
                 bodyMapping[noConstraint+i][0] = constraint->getBody1();
                 bodyMapping[noConstraint+i][1] = constraint->getBody2();
+
+                // Fill in the init lambda value for the constraint
+                lambdaInit.setValue(noConstraint+i, 0.0);
             }
 
             // Fill in the limit vectors for the auxilirary constraints
@@ -263,6 +286,29 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
     }
 }
 
+// Clear and Fill in the contact cache with the new lambda values
+void ConstraintSolver::updateContactCache() {
+    // Clear the contact cache
+    contactCache.clear();
+    
+    // For each active constraint
+    uint noConstraint = 0;
+    for (uint c=0; c<activeConstraints.size(); c++) {
+
+        // If it's a contact constraint
+        Contact* contact = dynamic_cast<Contact*>(activeConstraints.at(c));
+        if (contact) {
+            // Create a new ContactCachingInfo
+            ContactCachingInfo contactInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
+
+            // Add it to the contact cache
+            contactCache.addContactCachingInfo(contactInfo);
+        }
+
+        noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints();
+    }
+}
+
 // Solve the current LCP problem
 void ConstraintSolver::solve(double dt) {
     // Allocate memory for the matrices
@@ -278,11 +324,12 @@ void ConstraintSolver::solve(double dt) {
     computeMatrixB_sp();
 
     // Solve the LCP problem (computation of lambda)
-    Vector lambdaInit(nbConstraints);
-    lambdaInit.initWithValue(0.0);
     lcpSolver->setLambdaInit(lambdaInit);
     lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
 
+    // Update the contact chaching informations
+    updateContactCache();
+
     // Compute the vector Vconstraint
     computeVectorVconstraint(dt);
 }

sources/reactphysics3d/engine/ConstraintSolver.h

@@ -24,7 +24,7 @@
 #include "../typeDefinitions.h"
 #include "../constraint/Constraint.h"
 #include "../mathematics/lcp/LCPSolver.h"
-#include "../integration/IntegrationAlgorithm.h"    // TODO : Delete this
+#include "ContactCache.h"
 #include "PhysicsWorld.h"
 #include <map>
 #include <set>
@@ -46,6 +46,7 @@ class ConstraintSolver {
     protected:
         PhysicsWorld* physicsWorld;                             // Reference to the physics world
         LCPSolver* lcpSolver;                                   // LCP Solver
+        ContactCache contactCache;                              // Contact cache
         std::vector<Constraint*> activeConstraints;             // Current active constraints in the physics world
         uint nbConstraints;                                     // Total number of constraints (with the auxiliary constraints)
         std::set<Body*> constraintBodies;                       // Bodies that are implied in some constraint
@@ -63,6 +64,7 @@ class ConstraintSolver {
                                                                 // a 6x1 matrix
         Vector b;                                               // Vector "b" of the LCP problem
         Vector lambda;                                          // Lambda vector of the LCP problem
+        Vector lambdaInit;                                      // Lambda init vector for the LCP solver
         Vector errorValues;                                     // Error vector of all constraints
         Vector lowerBounds;                                     // Vector that contains the low limits for the variables of the LCP problem
         Vector upperBounds;                                     // Vector that contains the high limits for the variables of the LCP problem
@@ -78,6 +80,7 @@ class ConstraintSolver {
         void computeVectorB(double dt);                         // Compute the vector b
         void computeMatrixB_sp();                               // Compute the matrix B_sp
         void computeVectorVconstraint(double dt);               // Compute the vector V2
+        void updateContactCache();                              // Clear and Fill in the contact cache with the new lambda values
 
     public:
         ConstraintSolver(PhysicsWorld* world);                                  // Constructor

sources/reactphysics3d/engine/ContactCache.cpp

@@ -0,0 +1,87 @@
+/***************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+// Libraries
+#include "ContactCache.h"
+
+using namespace reactphysics3d;
+using namespace std;
+
+// Constructor
+ContactCache::ContactCache() {
+
+}
+
+// Destructor
+ContactCache::~ContactCache() {
+    
+}
+
+// Remove all the contact caching info of the cache
+void ContactCache::clear() {
+    // Clear the cache
+    cache.clear();
+}
+
+// Add a new contact caching info in the cache
+void ContactCache::addContactCachingInfo(const ContactCachingInfo& info) {
+    // Check if there is already an entry for that pair of body in the cache
+    map<pair<Body*, Body*>, vector<ContactCachingInfo> >::iterator entry = cache.find(make_pair(info.body1, info.body2));
+    if (entry != cache.end()) {
+        (*entry).second.push_back(info);
+    }
+    else {
+        // Add a new entry in the cache
+        vector<ContactCachingInfo> vec;
+        vec.push_back(info);
+        cache.insert(make_pair(make_pair(info.body1, info.body2), vec));
+    }
+}
+
+// Return the ContactCachingInfo (if exists) corresponding to the arguments
+// If the contact pair between the two bodies in argument doesn't exist this or there is no ContactCachingInfo
+// compatible (with approximatively the same position), this method returns NULL.
+ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) {
+   // Check if there is an entry for that pair of body in the cache
+    map<pair<Body*, Body*>, vector<ContactCachingInfo> >::iterator entry = cache.find(make_pair(body1, body2));
+    if (entry != cache.end()) {
+        vector<ContactCachingInfo> vec = (*entry).second;
+        assert((*entry).first.first == body1);
+        assert((*entry).first.second == body2);
+
+        double posX = position.getX();
+        double posY = position.getY();
+        double posZ = position.getZ();
+
+        // Check among all the old contacts for this pair of body if there is an approximative match for the contact position
+        for(vector<ContactCachingInfo>::iterator it = vec.begin(); it != vec.end(); it++) {
+            Vector3D& contactPos = (*it).position;
+            if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE &&
+                posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE &&
+                posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) {
+                // Return the ContactCachingInfo
+                return &(*it);
+            }
+        }
+    }
+    else {
+        return 0;
+    }
+}
+

sources/reactphysics3d/engine/ContactCache.h

@@ -0,0 +1,58 @@
+/***************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+#ifndef CONTACTCACHE_H
+#define	CONTACTCACHE_H
+
+// Libraries
+#include <map>
+#include <vector>
+#include <utility>
+#include "ContactCachingInfo.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Constant
+const double POSITION_TOLERANCE = 0.01 ;      // Tolerance used to consider two similar positions to be considered as the same
+
+/*  -------------------------------------------------------------------
+    Class ContactCache :
+       This class is used to cache the contact in order to be able to
+       use the lambda values in the last time step in the next time step in the
+       constraint solver to improve the convergence rate of the PGS algorithm.
+       This class will cache the ContactCachingInfo objects at each time step.
+    -------------------------------------------------------------------
+*/
+class ContactCache {
+    private:
+        std::map<std::pair<Body*, Body*>, std::vector<ContactCachingInfo> > cache;
+
+    public:
+        ContactCache();                                                                                         // Constructor
+        ~ContactCache();                                                                                        // Destructor
+        void clear();                                                                                           // Remove all the contact caching info of the cache
+        void addContactCachingInfo(const ContactCachingInfo& contactCachingInfo);                               // Add a new contact caching info in the cache
+        ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position);    // Return the ContactCachingInfo (if exists) corresponding to the arguments
+};
+
+} // End of the ReactPhysics3D namespace
+
+#endif
+

sources/reactphysics3d/engine/ContactCachingInfo.cpp

@@ -0,0 +1,29 @@
+/***************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+// Libraries
+#include "ContactCachingInfo.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda)
+            : body1(body1), body2(body2), position(position), lambda(lambda) {
+
+}

sources/reactphysics3d/engine/ContactCachingInfo.h

@@ -0,0 +1,51 @@
+/***************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+#ifndef CONTACTCACHINGINFO_H
+#define	CONTACTCACHINGINFO_H
+
+// Libraries
+#include "../body/OBB.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+/*  -------------------------------------------------------------------
+    Structure ContactCachingInfo :
+       This structure contains informations used to cache the last lambda
+       value of each contact constraint. The last lambda value is used
+       as the init lambda value in the constraint solver in the next
+       time step to improve the convergence rate of the constraint solver.
+    -------------------------------------------------------------------
+*/
+struct ContactCachingInfo {
+    public:
+        // TODO : Use polymorphism here (change OBB into BoundingVolume to be more general)
+        Body* body1;                            // Body pointer of the first bounding volume
+        Body* body2;                            // Body pointer of the second bounding volume
+        Vector3D position;                       // Contact point
+        double lambda;                           // Last lambda value for the constraint
+
+        ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda);   // Constructor
+};
+
+} // End of the ReactPhysics3D namespace
+
+#endif
+

sources/reactphysics3d/engine/PhysicsEngine.cpp

@@ -19,7 +19,6 @@
 
 // Libraries
 #include "PhysicsEngine.h"
-#include "../integration/SemiImplicitEuler.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;

sources/reactphysics3d/engine/PhysicsEngine.h

@@ -22,7 +22,6 @@
 
 // Libraries
 #include "PhysicsWorld.h"
-#include "../integration/IntegrationAlgorithm.h"
 #include "../collision/CollisionDetection.h"
 #include "ConstraintSolver.h"
 #include "../body/RigidBody.h"