From 29d9e60e93197980cb9bdd2416458fd8eddc50ef Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Fri, 11 Nov 2011 07:05:30 +0000 Subject: [PATCH] Avoid negative penetration depth due to numerical errors in GJK algorithm git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@451 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../narrowphase/GJK/GJKAlgorithm.cpp | 16 +++ src/engine/ConstraintSolver.cpp | 1 - src/engine/ConstraintSolver.h | 16 --- src/engine/ContactCache.cpp | 109 ------------------ src/engine/ContactCache.h | 67 ----------- src/engine/ContactCachingInfo.cpp | 35 ------ src/engine/ContactCachingInfo.h | 57 --------- 7 files changed, 16 insertions(+), 285 deletions(-) delete mode 100644 src/engine/ContactCache.cpp delete mode 100644 src/engine/ContactCache.h delete mode 100644 src/engine/ContactCachingInfo.cpp delete mode 100644 src/engine/ContactCachingInfo.h diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 7f01e0bd..6b85370b 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -126,6 +126,10 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); 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); // 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 Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); 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); // 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 Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); 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); // 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 Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); 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); // There is an intersection, therefore we return true diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index fd63b82a..9759dedb 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -24,7 +24,6 @@ // Libraries #include "ConstraintSolver.h" -#include "../mathematics/lcp/LCPProjectedGaussSeidel.h" #include "../body/RigidBody.h" using namespace reactphysics3d; diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 72e07ee4..ec58c852 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -28,12 +28,9 @@ // Libraries #include "../constants.h" #include "../constraint/Constraint.h" -#include "../mathematics/lcp/LCPSolver.h" -#include "ContactCache.h" #include "PhysicsWorld.h" #include #include -#include // TODO : Remove this // ReactPhysics3D namespace namespace reactphysics3d { @@ -147,12 +144,6 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { // Solve the current LCP problem 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 initialize(); @@ -174,13 +165,6 @@ inline void ConstraintSolver::solve(double dt) { // Compute the vector Vconstraint 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 diff --git a/src/engine/ContactCache.cpp b/src/engine/ContactCache.cpp deleted file mode 100644 index 2131c3ed..00000000 --- a/src/engine/ContactCache.cpp +++ /dev/null @@ -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, 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; - } -} - diff --git a/src/engine/ContactCache.h b/src/engine/ContactCache.h deleted file mode 100644 index 72fe9213..00000000 --- a/src/engine/ContactCache.h +++ /dev/null @@ -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 -#include -#include -#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, 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 - diff --git a/src/engine/ContactCachingInfo.cpp b/src/engine/ContactCachingInfo.cpp deleted file mode 100644 index 11f7353a..00000000 --- a/src/engine/ContactCachingInfo.cpp +++ /dev/null @@ -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& positions, const vector& lambdas) - : body1(body1), body2(body2), positions(positions), lambdas(lambdas) { - -} diff --git a/src/engine/ContactCachingInfo.h b/src/engine/ContactCachingInfo.h deleted file mode 100644 index 3ad8b93a..00000000 --- a/src/engine/ContactCachingInfo.h +++ /dev/null @@ -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 positions; // Positions of the contact points - std::vector lambdas; // Last lambdas value for the constraint - - ContactCachingInfo(Body* body1, Body* body2, const std::vector& positions, const std::vector& lambdas); // Constructor -}; - -} // End of the ReactPhysics3D namespace - -#endif -