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

Avoid negative penetration depth due to numerical errors in GJK algorithm

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@451 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-11-11 07:05:30 +00:00
parent ab6865b3a1
commit 29d9e60e93
7 changed files with 16 additions and 285 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -126,6 +126,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
@ -150,6 +154,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
@ -172,6 +180,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
@ -202,6 +214,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Compute the contact info // Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true

1
src/engine/ConstraintSolver.cpp
View File

@ -24,7 +24,6 @@
// Libraries // Libraries
#include "ConstraintSolver.h" #include "ConstraintSolver.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
using namespace reactphysics3d; using namespace reactphysics3d;

16
src/engine/ConstraintSolver.h
View File

@ -28,12 +28,9 @@
// Libraries // Libraries
#include "../constants.h" #include "../constants.h"
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPSolver.h"
#include "ContactCache.h"
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include <map> #include <map>
#include <set> #include <set>
#include <sys/time.h> // TODO : Remove this
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -148,12 +145,6 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
// Solve the current LCP problem // Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) { inline void ConstraintSolver::solve(double dt) {
// TODO : Remove the following timing code
timeval timeValueStart;
timeval timeValueEnd;
std::cout << "------ START (Constraint Solver) -----" << std::endl;
gettimeofday(&timeValueStart, NULL);
// Allocate memory for the matrices // Allocate memory for the matrices
initialize(); initialize();
@ -174,13 +165,6 @@ inline void ConstraintSolver::solve(double dt) {
// Compute the vector Vconstraint // Compute the vector Vconstraint
computeVectorVconstraint(dt); computeVectorVconstraint(dt);
// TODO : Remove the following timing code
std::cout << "NB constraints : " << nbConstraints << std::endl;
gettimeofday(&timeValueEnd, NULL);
long double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
long double endTime = timeValueEnd.tv_sec * 1000000.0 + (timeValueEnd.tv_usec);
std::cout << "------ END (Constraint Solver) => (" << "time = " << endTime - startTime << " micro sec)-----" << std::endl;
} }
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace

109
src/engine/ContactCache.cpp
View File

@ -1,109 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// 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(ContactCachingInfo* info) {
// Add a new entry in the cache
cache.insert(make_pair(make_pair(info->body1, info->body2), info));
}
// 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(Contact* contact) const {
double posX, posY, posZ;
// Check if there is an entry for that pair of body in the cache
map<pair<Body*, Body*>, ContactCachingInfo* >::const_iterator entry = cache.find(make_pair(contact->getBody1(), contact->getBody2()));
if (entry != cache.end()) {
ContactCachingInfo* contactInfo = (*entry).second;
assert(contactInfo);
assert((*entry).first.first == contact->getBody1());
assert((*entry).first.second == contact->getBody2());
// Get the position of the current contact
posX = contact->getWorldPointOnBody1().getX();
posY = contact->getWorldPointOnBody1().getY();
posZ = contact->getWorldPointOnBody1().getZ();
// Get the position of the old contact
Vector3& contactPos1 = contactInfo->positions[0];
// If the old contact point doesn't match the current one
if (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE ||
posY > contactPos1.getY() + POSITION_TOLERANCE || posY < contactPos1.getY() - POSITION_TOLERANCE ||
posZ > contactPos1.getZ() + POSITION_TOLERANCE || posZ < contactPos1.getZ() - POSITION_TOLERANCE) {
// Return NULL
return NULL;
}
// Get the position of the current contact
posX = contact->getWorldPointOnBody2().getX();
posY = contact->getWorldPointOnBody2().getY();
posZ = contact->getWorldPointOnBody2().getZ();
// Get the position of the old contact
Vector3& contactPos2 = contactInfo->positions[1];
// If the old contact point doesn't match the current one
if (posX > contactPos2.getX() + POSITION_TOLERANCE || posX < contactPos2.getX() - POSITION_TOLERANCE ||
posY > contactPos2.getY() + POSITION_TOLERANCE || posY < contactPos2.getY() - POSITION_TOLERANCE ||
posZ > contactPos2.getZ() + POSITION_TOLERANCE || posZ < contactPos2.getZ() - POSITION_TOLERANCE) {
// Return NULL
return NULL;
}
// The old contact positions match the current contact, therefore we return the contact caching info
return contactInfo;
}
else {
return NULL;
}
}

67
src/engine/ContactCache.h
View File

@ -1,67 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACT_CACHE_H
#define CONTACT_CACHE_H
// Libraries
#include <map>
#include <vector>
#include <utility>
#include "ContactCachingInfo.h"
#include "../constraint/Contact.h"
// TODO : Remove this class
// 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*>, ContactCachingInfo* > cache;
public:
ContactCache(); // Constructor
~ContactCache(); // Destructor
void clear(); // Remove all the contact caching info of the cache
ContactCachingInfo* getContactCachingInfo(Contact* contact) const; // Return the ContactCachingInfo corresponding to a contact if it exists
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
};
} // End of the ReactPhysics3D namespace
#endif

35
src/engine/ContactCachingInfo.cpp
View File

@ -1,35 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "ContactCachingInfo.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector<Vector3>& positions, const vector<double>& lambdas)
: body1(body1), body2(body2), positions(positions), lambdas(lambdas) {
}

57
src/engine/ContactCachingInfo.h
View File

@ -1,57 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACT_CACHING_INFO_H
#define CONTACT_CACHING_INFO_H
// TODO : Remove this class
// Libraries
#include "../shapes/BoxShape.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:
Body* body1; // Body pointer of the first bounding volume
Body* body2; // Body pointer of the second bounding volume
std::vector<Vector3> positions; // Positions of the contact points
std::vector<double> lambdas; // Last lambdas value for the constraint
ContactCachingInfo(Body* body1, Body* body2, const std::vector<Vector3>& positions, const std::vector<double>& lambdas); // Constructor
};
} // End of the ReactPhysics3D namespace
#endif