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

Add the PersistentContactCache and MemoryPool classes

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@439 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-09-03 11:58:42 +00:00
parent 6c6b6c6c86
commit 94e7153817
15 changed files with 592 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/collision/CollisionDetection.h
View File

@ -55,7 +55,7 @@ class CollisionDetection {
void computeBroadPhase(); // Compute the broad-phase collision detection void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection void computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list void computeAllContacts(); // Compute all the contacts from the collision info list
void computeContactSAT(const ContactInfo* const contactInfo); // Compute a contact for SAT (and add it to the physics world) for two colliding bodies //void computeContactSAT(const ContactInfo* const contactInfo); // Compute a contact for SAT (and add it to the physics world) for two colliding bodies
void computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world) void computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world)
public : public :

14
src/collision/ContactInfo.cpp
View File

@ -27,16 +27,12 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor for SAT
// TODO : DELETE THIS
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) {
}
// Constructor for GJK // Constructor for GJK
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
double penetrationDepth, const Vector3& point1, const Vector3& point2) const Vector3& localPoint1, const Vector3& localPoint2,
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) { const Transform& transform1, const Transform& transform2)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth),
localPoint1(localPoint1), localPoint2(localPoint2), worldPoint1(transform1 * localPoint1), worldPoint2(transform2 * localPoint2) {
} }

15
src/collision/ContactInfo.h
View File

@ -44,15 +44,16 @@ struct ContactInfo {
public: public:
Body* const body1; // Pointer to the first body of the contact Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact Body* const body2; // Pointer to the second body of the contact
const Vector3 point1; // Contact point of body 1 const Vector3 localPoint1; // Contact point of body 1 in local space of body 1
const Vector3 point2; // Contact point of body 2 const Vector3 localPoint2; // Contact point of body 2 in local space of body 2
const Vector3 normal; // Normal vector the the collision contact const Vector3 worldPoint1; // Contact point of body 1 in world space
const Vector3 worldPoint2; // Contact point of body 2 in world space
const Vector3 normal; // Normal vector the the collision contact in world space
const double penetrationDepth; // Penetration depth of the contact const double penetrationDepth; // Penetration depth of the contact
ContactInfo(Body* body1, Body* body2, const Vector3& normal, ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
double penetrationDepth); // Constructor for SAT const Vector3& localPoint1, const Vector3& localPoint2,
ContactInfo(Body* body1, Body* body2, const Vector3& normal, const Transform& transform1, const Transform& transform2); // Constructor for GJK
double penetrationDepth, const Vector3& point1, const Vector3& point2); // Constructor for GJK
}; };
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

9
src/collision/EPA/EPAAlgorithm.cpp
View File

@ -366,14 +366,15 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} }
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info (in world-space) // Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint(); v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3 pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB); Vector3 pBLocal = shape2ToShape1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit(); Vector3 normal = v.getUnit();
double penetrationDepth = v.length(); double penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal,
penetrationDepth, pALocal, pBLocal, transform1, transform2);
return true; return true;
} }

24
src/collision/GJK/GJKAlgorithm.cpp
View File

@ -119,13 +119,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v); pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// 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;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); 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
return true; return true;
@ -143,13 +143,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v); pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// 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;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); 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
return true; return true;
@ -165,13 +165,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v); pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// 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;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); 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
return true; return true;
@ -195,13 +195,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins // object with the margins
double dist = sqrt(distSquare); double dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v); pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// 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;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); 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
return true; return true;

2
src/collision/SATAlgorithm.cpp
View File

@ -33,6 +33,7 @@
#include <cmath> #include <cmath>
#include <cassert> #include <cassert>
/*
// TODO : SAT Algorithm for box-box collision does not work anymore since the use of // TODO : SAT Algorithm for box-box collision does not work anymore since the use of
// transform. Maybe a problem with the computation of the normal vector // transform. Maybe a problem with the computation of the normal vector
// Anyway, the GJK algorithm should be used instead // Anyway, the GJK algorithm should be used instead
@ -426,3 +427,4 @@ double SATAlgorithm::computePenetrationDepth(double min1, double max1, double mi
// Compute the current penetration depth // Compute the current penetration depth
return (lengthInterval1 + lengthInterval2) - lengthBothIntervals; return (lengthInterval1 + lengthInterval2) - lengthBothIntervals;
} }
*/

1
src/constants.h
View File

@ -50,6 +50,7 @@ const double OBJECT_MARGIN = 0.04; // Object margin for collision detec
const double FRICTION_COEFFICIENT = 1.0; // Friction coefficient const double FRICTION_COEFFICIENT = 1.0; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact // penetration depth in order to calculate the correct impulse for the contact
const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact
// Constraint solver constants // Constraint solver constants
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem

8
src/constraint/Contact.cpp
View File

@ -31,7 +31,9 @@ using namespace std;
// Constructor // Constructor
Contact::Contact(const ContactInfo* contactInfo) Contact::Contact(const ContactInfo* contactInfo)
: Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth), : Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
pointOnBody1(contactInfo->point1), pointOnBody2(contactInfo->point2) { localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2),
worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) {
assert(penetrationDepth > 0.0);
// Compute the auxiliary lower and upper bounds // Compute the auxiliary lower and upper bounds
// TODO : Now mC is only the mass of the first body but it is probably wrong // TODO : Now mC is only the mass of the first body but it is probably wrong
@ -60,8 +62,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3 body2Position = body2->getTransform().getPosition(); Vector3 body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index int currentIndex = noConstraint; // Current constraint index
Vector3 r1 = pointOnBody1 - body1Position; Vector3 r1 = worldPointOnBody1 - body1Position;
Vector3 r2 = pointOnBody2 - body2Position; Vector3 r2 = worldPointOnBody2 - body2Position;
Vector3 r1CrossN = r1.cross(normal); Vector3 r1CrossN = r1.cross(normal);
Vector3 r2CrossN = r2.cross(normal); Vector3 r2CrossN = r2.cross(normal);

52
src/constraint/Contact.h
View File

@ -51,10 +51,12 @@ namespace reactphysics3d {
*/ */
class Contact : public Constraint { class Contact : public Constraint {
protected : protected :
const Vector3 normal; // Normal vector of the contact (From body1 toward body2) const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space
const double penetrationDepth; // Penetration depth double penetrationDepth; // Penetration depth
const Vector3 pointOnBody1; // Contact point on body 1 const Vector3 localPointOnBody1; // Contact point on body 1 in local space of body 1
const Vector3 pointOnBody2; // Contact point on body 2 const Vector3 localPointOnBody2; // Contact point on body 2 in local space of body 2
Vector3 worldPointOnBody1; // Contact point on body 1 in world space
Vector3 worldPointOnBody2; // Contact point on body 2 in world space
std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
double mu_mc_g; double mu_mc_g;
@ -65,8 +67,13 @@ class Contact : public Constraint {
virtual ~Contact(); // Destructor virtual ~Contact(); // Destructor
Vector3 getNormal() const; // Return the normal vector of the contact Vector3 getNormal() const; // Return the normal vector of the contact
Vector3 getPointOnBody1() const; // Return the contact point on body 1 void setPenetrationDepth(double penetrationDepth); // Set the penetration depth of the contact
Vector3 getPointOnBody2() const; // Return the contact point on body 2 Vector3 getLocalPointOnBody1() const; // Return the contact local point on body 1
Vector3 getLocalPointOnBody2() const; // Return the contact local point on body 2
Vector3 getWorldPointOnBody1() const; // Return the contact world point on body 1
Vector3 getWorldPointOnBody2() const; // Return the contact world point on body 2
void setWorldPointOnBody1(const Vector3& worldPoint); // Set the contact world point on body 1
void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints
@ -96,14 +103,39 @@ inline Vector3 Contact::getNormal() const {
return normal; return normal;
} }
// Set the penetration depth of the contact
inline void Contact::setPenetrationDepth(double penetrationDepth) {
this->penetrationDepth = penetrationDepth;
}
// Return the contact point on body 1 // Return the contact point on body 1
inline Vector3 Contact::getPointOnBody1() const { inline Vector3 Contact::getLocalPointOnBody1() const {
return pointOnBody1; return localPointOnBody1;
} }
// Return the contact point on body 2 // Return the contact point on body 2
inline Vector3 Contact::getPointOnBody2() const { inline Vector3 Contact::getLocalPointOnBody2() const {
return pointOnBody2; return localPointOnBody2;
}
// Return the contact world point on body 1
inline Vector3 Contact::getWorldPointOnBody1() const {
return worldPointOnBody1;
}
// Return the contact world point on body 2
inline Vector3 Contact::getWorldPointOnBody2() const {
return worldPointOnBody2;
}
// Set the contact world point on body 1
inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) {
worldPointOnBody1 = worldPoint;
}
// Set the contact world point on body 2
inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) {
worldPointOnBody2 = worldPoint;
} }
// Return the penetration depth of the contact // Return the penetration depth of the contact

4
src/engine/ConstraintSolver.cpp
View File

@ -357,8 +357,8 @@ void ConstraintSolver::updateContactCache() {
// Get all the contact points of the contact // Get all the contact points of the contact
vector<Vector3> points; vector<Vector3> points;
vector<double> lambdas; vector<double> lambdas;
points.push_back(contact->getPointOnBody1()); points.push_back(contact->getWorldPointOnBody1());
points.push_back(contact->getPointOnBody2()); points.push_back(contact->getWorldPointOnBody2());
// For each constraint of the contact // For each constraint of the contact
for (int i=0; i<contact->getNbConstraints(); i++) { for (int i=0; i<contact->getNbConstraints(); i++) {

12
src/engine/ContactCache.cpp
View File

@ -66,9 +66,9 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
assert((*entry).first.second == contact->getBody2()); assert((*entry).first.second == contact->getBody2());
// Get the position of the current contact // Get the position of the current contact
posX = contact->getPointOnBody1().getX(); posX = contact->getWorldPointOnBody1().getX();
posY = contact->getPointOnBody1().getY(); posY = contact->getWorldPointOnBody1().getY();
posZ = contact->getPointOnBody1().getZ(); posZ = contact->getWorldPointOnBody1().getZ();
// Get the position of the old contact // Get the position of the old contact
Vector3& contactPos1 = contactInfo->positions[0]; Vector3& contactPos1 = contactInfo->positions[0];
@ -83,9 +83,9 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
} }
// Get the position of the current contact // Get the position of the current contact
posX = contact->getPointOnBody2().getX(); posX = contact->getWorldPointOnBody2().getX();
posY = contact->getPointOnBody2().getY(); posY = contact->getWorldPointOnBody2().getY();
posZ = contact->getPointOnBody2().getZ(); posZ = contact->getWorldPointOnBody2().getZ();
// Get the position of the old contact // Get the position of the old contact
Vector3& contactPos2 = contactInfo->positions[1]; Vector3& contactPos2 = contactInfo->positions[1];

201
src/engine/PersistentContactCache.cpp Normal file
View File

@ -0,0 +1,201 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 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 "PersistentContactCache.h"
using namespace reactphysics3d;
// Constructor
PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2)
: body1(body1), body2(body2), nbContacts(0) {
}
// Destructor
PersistentContactCache::~PersistentContactCache() {
clear();
}
// Add a contact in the cache
void PersistentContactCache::addContact(Contact* contact) {
int indexNewContact = nbContacts;
// If the contact cache is full
if (nbContacts == MAX_CONTACTS_IN_CACHE) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContact(indexToRemove);
indexNewContact = indexToRemove;
}
// Add the new contact in the cache
contacts[indexNewContact] = contact;
nbContacts++;
}
// Remove a contact from the cache
void PersistentContactCache::removeContact(int index) {
assert(index >= 0 && index < nbContacts);
assert(nbContacts > 0);
delete contacts[index];
// If we don't remove the last index
if (index < nbContacts - 1) {
contacts[index] = contacts[nbContacts - 1];
}
nbContacts--;
}
// Update the contact cache
// First the world space coordinates of the current contacts in the cache are recomputed from
// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
// the contacts with a too large distance between the contact points in the plane orthogonal to the
// contact normal
void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) {
// Update the world coordinates and penetration depth of the contacts in the cache
for (uint i=0; i<nbContacts; i++) {
contacts[i]->setWorldPointOnBody1(transform1 * contacts[i]->getLocalPointOnBody1());
contacts[i]->setWorldPointOnBody2(transform2 * contacts[i]->getLocalPointOnBody2());
contacts[i]->setPenetrationDepth((contacts[i]->getWorldPointOnBody1() - contacts[i]->getWorldPointOnBody2()).dot(contacts[i]->getNormal()));
}
// Remove the contacts that don't represent very well the persistent contact
for (uint i=nbContacts-1; i>=0; i--) {
// Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
if (contacts[i]->getPenetrationDepth() <= 0.0) {
removeContact(i);
}
else {
// Compute the distance of the two contact points in the place orthogonal to the contact normal
Vector3 projOfPoint1 = contacts[i]->getWorldPointOnBody1() - contacts[i]->getNormal() * contacts[i]->getPenetrationDepth();
Vector3 projDifference = contacts[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance threshold, we remove the contact
if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) {
removeContact(i);
}
}
}
}
// Return the index of the contact with the larger penetration depth. This
// corresponding contact will be kept in the cache. The method returns -1 is
// the new contact is the deepest.
int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
assert(nbContacts == MAX_CONTACTS_IN_CACHE);
int indexMaxPenetrationDepth = -1;
double maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<nbContacts; i++) {
// If the current contact has a larger penetration depth
if (contacts[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = contacts[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed. The index of the contact with the larger penetration
// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
// the different area and we want to keep the contacts with the largest area. The new point is also
// kept.
int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(nbContacts == MAX_CONTACTS_IN_CACHE);
double area0 = 0.0; // Area with contact 1,2,3 and newPoint
double area1 = 0.0; // Area with contact 0,2,3 and newPoint
double area2 = 0.0; // Area with contact 0,1,3 and newPoint
double area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - contacts[1]->getLocalPointOnBody1();
Vector3 vector2 = contacts[3]->getLocalPointOnBody1() - contacts[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - contacts[0]->getLocalPointOnBody1();
Vector3 vector2 = contacts[3]->getLocalPointOnBody1() - contacts[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - contacts[0]->getLocalPointOnBody1();
Vector3 vector2 = contacts[3]->getLocalPointOnBody1() - contacts[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - contacts[0]->getLocalPointOnBody1();
Vector3 vector2 = contacts[2]->getLocalPointOnBody1() - contacts[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int PersistentContactCache::getMaxArea(double area0, double area1, double area2, double area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area0 < area3) return 3;
else return 0;
}
}
}
// Clear the cache
void PersistentContactCache::clear() {
for (uint i=0; i<nbContacts; i++) {
delete contacts[i];
}
nbContacts = 0;
}

74
src/engine/PersistentContactCache.h Normal file
View File

@ -0,0 +1,74 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 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 PERSISTENT_CONTACT_CACHE_H
#define PERSISTENT_CONTACT_CACHE_H
// Libraries
#include <vector>
#include "../body/Body.h"
#include "../constraint/Contact.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache
/* -------------------------------------------------------------------
Class PersistentContactCache :
This class represents a cache of at most 4 contact points between
two given bodies. The contacts between two bodies are added one
after the other in the cache. When the cache is full, we have
to remove one point. The idea is to keep the point with the deepest
penetration depth and also to keep the points producing the larger
area (for a more stable contact manifold). The new added point is
always kept. This kind of persistent cache has been explained for
instance in the presentation from Erin Catto about contact manifolds
at the GDC 2007 conference.
-------------------------------------------------------------------
*/
class PersistentContactCache {
private:
Body* const body1; // Pointer to the first body
Body* const body2; // Pointer to the second body
Contact* contacts[MAX_CONTACTS_IN_CACHE]; // Contacts in the cache
uint nbContacts; // Number of contacts in the cache
int getMaxArea(double area0, double area1, double area2, double area3) const; // Return the index of maximum area
int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed
void removeContact(int index); // Remove a contact from the cache
public:
PersistentContactCache(Body* const body1, Body* const body2); // Constructor
~PersistentContactCache(); // Destructor
void addContact(Contact* contact); // Add a contact
void update(const Transform& transform1, const Transform& transform2); // Update the contact cache
void clear(); // Clear the cache
};
}
#endif

124
src/memory/MemoryPool.cpp Normal file
View File

@ -0,0 +1,124 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 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 "MemoryPool.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
// Allocate a large block of memory in order to contain
// a given number of object of the template type T
template<typename T>
MemoryPool<T>::MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc) {
pMemoryBlock = NULL;
pAllocatedLinkedList = NULL;
pFreeLinkedList = NULL;
// Compute the total memory size that need to be allocated
memorySize = nbObjectsToAllocate * (sizeof(struct Unit) + sizeof(T));
// Allocate the whole memory block
pMemoryBlock = malloc(memorySize);
// Check that the allocation didn't fail
if (!pMemoryBlock) throw std::bad_alloc();
// For each allocated memory unit
for (uint i=0; i<nbObjectsToAllocate; i++) {
// Get the adress of a memory unit
struct Unit* currentUnit = (struct Unit*)( (char*)pMemoryBlock + i * (sizeof(T) + sizeof(struct Unit)) );
currentUnit->pPrevious = NULL;
currentUnit->pNext = pFreeLinkedList;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = currentUnit;
}
pFreeLinkedList = currentUnit;
}
}
// Destructor
// Deallocate the block of memory that has been allocated previously
template<typename T>
MemoryPool<T>::~MemoryPool() {
// Release the whole memory block
free(pMemoryBlock);
}
// Return a pointer to an memory allocated location to store a new object
// This method does not allocate memory because it has already been done at the
// beginning but it returns a pointer to a location in the allocated block of
// memory where a new object can be stored
template<typename T>
void* MemoryPool<T>::allocateObject() {
// If no memory unit is available in the current allocated memory block
if (!pFreeLinkedList) {
// TODO : REALLOCATE MEMORY HERE
assert(false);
}
assert(pFreeLinkedList);
struct Unit* currentUnit = pFreeLinkedList;
pFreeLinkedList = currentUnit->pNext;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = NULL;
}
currentUnit->pNext = pAllocatedLinkedList;
if (pAllocatedLinkedList) {
pAllocatedLinkedList->pPrevious = currentUnit;
}
pAllocatedLinkedList = currentUnit;
// Return a pointer to the allocated memory unit
return (void*)((char*)currentUnit + sizeof(struct Unit));
}
// Tell the pool that an object doesn't need to be store in the pool anymore
// This method does not deallocate memory because it will be done only at the
// end but it informs the memory pool that an object that was stored in the heap
// does not need to be stored anymore and therefore we can use the corresponding
// location in the pool for another object
template<typename T>
void MemoryPool<T>::freeObject(void* pObjectToFree) {
// The pointer location must be inside the memory block
assert(pMemoryBlock<pObjectToFree && pObjectToFree<(void*)((char*)pMemoryBlock + memorySize));
struct Unit* currentUnit = (struct Unit*)((char*)pObjectToFree - sizeof(struct Unit));
pAllocatedLinkedList = currentUnit->pNext;
if (pAllocatedLinkedList) {
pAllocatedLinkedList->pPrevious = NULL;
}
currentUnit->pNext = pFreeLinkedList;
if (pFreeLinkedList) {
pFreeLinkedList->pPrevious = currentUnit;
}
pFreeLinkedList = currentUnit;
}

70
src/memory/MemoryPool.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2011 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 MEMORY_POOL_H
#define MEMORY_POOL_H
// Libraries
#include "../constants.h"
#include <cstddef>
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class MemoryPool :
This class represents a memory pool that allows us to allocate
dynamic memory at the beginning of the application in order to
avoid memory fragmentation and also a large number of allocation
and deallocation.
-------------------------------------------------------------------
*/
template<typename T> // TODO : Check if we need to use a template here
class MemoryPool {
private:
// Unit of memory
struct Unit {
struct Unit* pNext; // Pointer to the next memory unit
struct Unit* pPrevious; // Pointer to the previous memory unit
};
void* pMemoryBlock; // Pointer to the whole memory block
struct Unit* pAllocatedLinkedList; // Pointer to the linked list of allocated memory units
struct Unit* pFreeLinkedList; // Pointer to the linked list of free memory units
size_t memorySize; // Total allocated memory in the pool
public:
MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc); // Constructor
~MemoryPool(); // Destructor
void* allocateObject(); // Return a pointer to an memory allocated location to store a new object
void freeObject(void* pObjectToFree); // Tell the pool that an object doesn't need to be store in the pool anymore
};
}
#endif