mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@365 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-07-23 12:51:26 +00:00
parent b22ed95c83
commit 8fbf330da5
8 changed files with 279 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -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);
}

7
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -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
@ -62,7 +63,8 @@ class ConstraintSolver {
// The dimension of this array is 2 times nbConstraints. Each cell will contain
// a 6x1 matrix
Vector b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector 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

87
sources/reactphysics3d/engine/ContactCache.cpp Normal file
View File

@ -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;
}
}

58
sources/reactphysics3d/engine/ContactCache.h Normal file
View File

@ -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

29
sources/reactphysics3d/engine/ContactCachingInfo.cpp Normal file
View File

@ -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) {
}

51
sources/reactphysics3d/engine/ContactCachingInfo.h Normal file
View File

@ -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

1
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

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

1
sources/reactphysics3d/engine/PhysicsEngine.h
View File

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