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
This commit is contained in:
parent
b22ed95c83
commit
8fbf330da5
|
@ -83,6 +83,7 @@ void ConstraintSolver::allocate() {
|
||||||
errorValues.changeSize(nbConstraints);
|
errorValues.changeSize(nbConstraints);
|
||||||
b.changeSize(nbConstraints);
|
b.changeSize(nbConstraints);
|
||||||
lambda.changeSize(nbConstraints);
|
lambda.changeSize(nbConstraints);
|
||||||
|
lambdaInit.changeSize(nbConstraints);
|
||||||
lowerBounds.changeSize(nbConstraints);
|
lowerBounds.changeSize(nbConstraints);
|
||||||
upperBounds.changeSize(nbConstraints);
|
upperBounds.changeSize(nbConstraints);
|
||||||
Minv_sp = new Matrix[nbBodies];
|
Minv_sp = new Matrix[nbBodies];
|
||||||
|
@ -119,6 +120,25 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
// Fill in the error vector
|
// Fill in the error vector
|
||||||
errorValues.setValue(noConstraint, constraint->getErrorValue());
|
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();
|
nbAuxConstraints = constraint->getNbAuxConstraints();
|
||||||
|
|
||||||
// If the current constraint has auxiliary constraints
|
// If the current constraint has auxiliary constraints
|
||||||
|
@ -135,6 +155,9 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
// Fill in the body mapping matrix
|
// Fill in the body mapping matrix
|
||||||
bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
||||||
bodyMapping[noConstraint+i][1] = constraint->getBody2();
|
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
|
// 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
|
// Solve the current LCP problem
|
||||||
void ConstraintSolver::solve(double dt) {
|
void ConstraintSolver::solve(double dt) {
|
||||||
// Allocate memory for the matrices
|
// Allocate memory for the matrices
|
||||||
|
@ -278,11 +324,12 @@ void ConstraintSolver::solve(double dt) {
|
||||||
computeMatrixB_sp();
|
computeMatrixB_sp();
|
||||||
|
|
||||||
// Solve the LCP problem (computation of lambda)
|
// Solve the LCP problem (computation of lambda)
|
||||||
Vector lambdaInit(nbConstraints);
|
|
||||||
lambdaInit.initWithValue(0.0);
|
|
||||||
lcpSolver->setLambdaInit(lambdaInit);
|
lcpSolver->setLambdaInit(lambdaInit);
|
||||||
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
|
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
|
||||||
|
|
||||||
|
// Update the contact chaching informations
|
||||||
|
updateContactCache();
|
||||||
|
|
||||||
// Compute the vector Vconstraint
|
// Compute the vector Vconstraint
|
||||||
computeVectorVconstraint(dt);
|
computeVectorVconstraint(dt);
|
||||||
}
|
}
|
||||||
|
|
|
@ -24,7 +24,7 @@
|
||||||
#include "../typeDefinitions.h"
|
#include "../typeDefinitions.h"
|
||||||
#include "../constraint/Constraint.h"
|
#include "../constraint/Constraint.h"
|
||||||
#include "../mathematics/lcp/LCPSolver.h"
|
#include "../mathematics/lcp/LCPSolver.h"
|
||||||
#include "../integration/IntegrationAlgorithm.h" // TODO : Delete this
|
#include "ContactCache.h"
|
||||||
#include "PhysicsWorld.h"
|
#include "PhysicsWorld.h"
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <set>
|
#include <set>
|
||||||
|
@ -46,6 +46,7 @@ class ConstraintSolver {
|
||||||
protected:
|
protected:
|
||||||
PhysicsWorld* physicsWorld; // Reference to the physics world
|
PhysicsWorld* physicsWorld; // Reference to the physics world
|
||||||
LCPSolver* lcpSolver; // LCP Solver
|
LCPSolver* lcpSolver; // LCP Solver
|
||||||
|
ContactCache contactCache; // Contact cache
|
||||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||||
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
|
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
|
||||||
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
|
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
|
// The dimension of this array is 2 times nbConstraints. Each cell will contain
|
||||||
// a 6x1 matrix
|
// a 6x1 matrix
|
||||||
Vector b; // Vector "b" of the LCP problem
|
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 errorValues; // Error vector of all constraints
|
||||||
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
|
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
|
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 computeVectorB(double dt); // Compute the vector b
|
||||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||||
void computeVectorVconstraint(double dt); // Compute the vector V2
|
void computeVectorVconstraint(double dt); // Compute the vector V2
|
||||||
|
void updateContactCache(); // Clear and Fill in the contact cache with the new lambda values
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ConstraintSolver(PhysicsWorld* world); // Constructor
|
ConstraintSolver(PhysicsWorld* world); // Constructor
|
||||||
|
|
87
sources/reactphysics3d/engine/ContactCache.cpp
Normal file
87
sources/reactphysics3d/engine/ContactCache.cpp
Normal 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
58
sources/reactphysics3d/engine/ContactCache.h
Normal 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
29
sources/reactphysics3d/engine/ContactCachingInfo.cpp
Normal 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
51
sources/reactphysics3d/engine/ContactCachingInfo.h
Normal 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
|
||||||
|
|
|
@ -19,7 +19,6 @@
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "PhysicsEngine.h"
|
#include "PhysicsEngine.h"
|
||||||
#include "../integration/SemiImplicitEuler.h"
|
|
||||||
|
|
||||||
// We want to use the ReactPhysics3D namespace
|
// We want to use the ReactPhysics3D namespace
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "PhysicsWorld.h"
|
#include "PhysicsWorld.h"
|
||||||
#include "../integration/IntegrationAlgorithm.h"
|
|
||||||
#include "../collision/CollisionDetection.h"
|
#include "../collision/CollisionDetection.h"
|
||||||
#include "ConstraintSolver.h"
|
#include "ConstraintSolver.h"
|
||||||
#include "../body/RigidBody.h"
|
#include "../body/RigidBody.h"
|
||||||
|
|
Loading…
Reference in New Issue
Block a user