diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index b5b6920f..057d44cc 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/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(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.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); } diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index a0966588..3cf71743 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/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 #include @@ -46,6 +46,7 @@ class ConstraintSolver { protected: PhysicsWorld* physicsWorld; // Reference to the physics world LCPSolver* lcpSolver; // LCP Solver + ContactCache contactCache; // Contact cache std::vector activeConstraints; // Current active constraints in the physics world uint nbConstraints; // Total number of constraints (with the auxiliary constraints) std::set 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 diff --git a/sources/reactphysics3d/engine/ContactCache.cpp b/sources/reactphysics3d/engine/ContactCache.cpp new file mode 100644 index 00000000..8fa3c14e --- /dev/null +++ b/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 . * +***************************************************************************/ + +// 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, vector >::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 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, vector >::iterator entry = cache.find(make_pair(body1, body2)); + if (entry != cache.end()) { + vector 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::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; + } +} + diff --git a/sources/reactphysics3d/engine/ContactCache.h b/sources/reactphysics3d/engine/ContactCache.h new file mode 100644 index 00000000..950952cf --- /dev/null +++ b/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 . * +***************************************************************************/ + +#ifndef CONTACTCACHE_H +#define CONTACTCACHE_H + +// Libraries +#include +#include +#include +#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::vector > 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 + diff --git a/sources/reactphysics3d/engine/ContactCachingInfo.cpp b/sources/reactphysics3d/engine/ContactCachingInfo.cpp new file mode 100644 index 00000000..67b49e12 --- /dev/null +++ b/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 . * +***************************************************************************/ + +// 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) { + +} diff --git a/sources/reactphysics3d/engine/ContactCachingInfo.h b/sources/reactphysics3d/engine/ContactCachingInfo.h new file mode 100644 index 00000000..99ef5589 --- /dev/null +++ b/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 . * +***************************************************************************/ + +#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 + diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index a4619ab2..e6d04160 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/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; diff --git a/sources/reactphysics3d/engine/PhysicsEngine.h b/sources/reactphysics3d/engine/PhysicsEngine.h index 93f94ef2..571e8567 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.h +++ b/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"