diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 1e0aeb5b..461fff4e 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -46,23 +46,23 @@ namespace reactphysics3d {
*/
class CollisionDetection {
private :
- PhysicsWorld* world; // Pointer to the physics world
- std::vector<std::pair<Body*, Body*> > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
- std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
- BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
+ PhysicsWorld* world; // Pointer to the physics world
+ std::vector<std::pair<Body*, Body*> > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
+ std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
+ BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
- void computeBroadPhase(); // Compute the broad-phase collision detection
- void computeNarrowPhase(); // Compute the narrow-phase collision detection
- 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 computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world)
+ void computeBroadPhase(); // Compute the broad-phase collision detection
+ void computeNarrowPhase(); // Compute the narrow-phase collision detection
+ 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 computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world)
public :
- CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
- ~CollisionDetection(); // Destructor
+ CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
+ ~CollisionDetection(); // Destructor
- bool computeCollisionDetection(); // Compute the collision detection
+ bool computeCollisionDetection(); // Compute the collision detection
};
} // End of the ReactPhysics3D namespace
diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp
index d5964e31..079392ed 100644
--- a/src/collision/ContactInfo.cpp
+++ b/src/collision/ContactInfo.cpp
@@ -27,16 +27,12 @@
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
-ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal,
- double penetrationDepth, const Vector3& point1, const Vector3& point2)
- : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) {
-
+ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
+ const Vector3& localPoint1, const Vector3& localPoint2,
+ const Transform& transform1, const Transform& transform2)
+ : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth),
+ localPoint1(localPoint1), localPoint2(localPoint2), worldPoint1(transform1 * localPoint1), worldPoint2(transform2 * localPoint2) {
+
}
diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h
index e0771d09..bdb0102a 100644
--- a/src/collision/ContactInfo.h
+++ b/src/collision/ContactInfo.h
@@ -42,17 +42,18 @@ namespace reactphysics3d {
*/
struct ContactInfo {
public:
- Body* const body1; // Pointer to the first 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 point2; // Contact point of body 2
- const Vector3 normal; // Normal vector the the collision contact
- const double penetrationDepth; // Penetration depth 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
+ const Vector3 localPoint1; // Contact point of body 1 in local space of body 1
+ const Vector3 localPoint2; // Contact point of body 2 in local space of body 2
+ 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
- ContactInfo(Body* body1, Body* body2, const Vector3& normal,
- double penetrationDepth); // Constructor for SAT
- ContactInfo(Body* body1, Body* body2, const Vector3& normal,
- double penetrationDepth, const Vector3& point1, const Vector3& point2); // Constructor for GJK
+ ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
+ const Vector3& localPoint1, const Vector3& localPoint2,
+ const Transform& transform1, const Transform& transform2); // Constructor for GJK
};
} // End of the ReactPhysics3D namespace
diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp
index 1a55671f..9f0de3af 100644
--- a/src/collision/EPA/EPAAlgorithm.cpp
+++ b/src/collision/EPA/EPAAlgorithm.cpp
@@ -366,14 +366,15 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
- // Compute the contact info (in world-space)
+ // Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
- Vector3 pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
- Vector3 pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
+ Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
+ Vector3 pBLocal = shape2ToShape1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
double penetrationDepth = v.length();
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;
}
diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
index be34b2c0..d9bfdfc9 100644
--- a/src/collision/GJK/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -119,13 +119,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
- pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
- pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
+ pA = (pA - (OBJECT_MARGIN / dist) * v);
+ pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
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
return true;
@@ -143,13 +143,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
- pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
- pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
+ pA = (pA - (OBJECT_MARGIN / dist) * v);
+ pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
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
return true;
@@ -165,13 +165,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
- pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
- pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
+ pA = (pA - (OBJECT_MARGIN / dist) * v);
+ pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
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
return true;
@@ -195,13 +195,13 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
- pA = transform1 * (pA - (OBJECT_MARGIN / dist) * v);
- pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
+ pA = (pA - (OBJECT_MARGIN / dist) * v);
+ pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
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
return true;
diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp
index ccd5cc48..816743ad 100644
--- a/src/collision/SATAlgorithm.cpp
+++ b/src/collision/SATAlgorithm.cpp
@@ -33,6 +33,7 @@
#include <cmath>
#include <cassert>
+/*
// 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
// 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
return (lengthInterval1 + lengthInterval2) - lengthBothIntervals;
}
+*/
\ No newline at end of file
diff --git a/src/constants.h b/src/constants.h
index 87ea3e31..be9c4fe5 100644
--- a/src/constants.h
+++ b/src/constants.h
@@ -44,20 +44,21 @@ const double PI = 3.14159265; // P
const double DEFAULT_TIMESTEP = 0.002;
// GJK Algorithm parameters
-const double OBJECT_MARGIN = 0.04; // Object margin for collision detection
+const double OBJECT_MARGIN = 0.04; // Object margin for collision detection
// Contact constants
-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
- // penetration depth in order to calculate the correct impulse for the contact
+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
+ // 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
-const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
-const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter
-const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is
- // less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
- // is used to avoid to keep to much memory for a long time if the system doesn't
- // need that memory. This value is between 0.0 and 1.0
+const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
+const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter
+const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is
+ // less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
+ // is used to avoid to keep to much memory for a long time if the system doesn't
+ // need that memory. This value is between 0.0 and 1.0
#endif
diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp
index fc2442ee..ee0ebbd7 100644
--- a/src/constraint/Contact.cpp
+++ b/src/constraint/Contact.cpp
@@ -31,8 +31,10 @@ using namespace std;
// Constructor
Contact::Contact(const ContactInfo* contactInfo)
: 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
// TODO : Now mC is only the mass of the first body but it is probably wrong
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
@@ -60,8 +62,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3 body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index
- Vector3 r1 = pointOnBody1 - body1Position;
- Vector3 r2 = pointOnBody2 - body2Position;
+ Vector3 r1 = worldPointOnBody1 - body1Position;
+ Vector3 r2 = worldPointOnBody2 - body2Position;
Vector3 r1CrossN = r1.cross(normal);
Vector3 r2CrossN = r2.cross(normal);
diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h
index 7acef065..b9501edc 100644
--- a/src/constraint/Contact.h
+++ b/src/constraint/Contact.h
@@ -51,11 +51,13 @@ namespace reactphysics3d {
*/
class Contact : public Constraint {
protected :
- const Vector3 normal; // Normal vector of the contact (From body1 toward body2)
- const double penetrationDepth; // Penetration depth
- const Vector3 pointOnBody1; // Contact point on body 1
- const Vector3 pointOnBody2; // Contact point on body 2
- std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
+ const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space
+ double penetrationDepth; // Penetration depth
+ const Vector3 localPointOnBody1; // Contact point on body 1 in local space of body 1
+ 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
double mu_mc_g;
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
@@ -65,15 +67,20 @@ class Contact : public Constraint {
virtual ~Contact(); // Destructor
Vector3 getNormal() const; // Return the normal vector of the contact
- Vector3 getPointOnBody1() const; // Return the contact point on body 1
- Vector3 getPointOnBody2() const; // Return the contact point on body 2
- 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 computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints
- virtual void computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints
- double getPenetrationDepth() const; // Return the penetration depth
+ void setPenetrationDepth(double penetrationDepth); // Set the penetration depth of the contact
+ 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 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 computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints
+ double getPenetrationDepth() const; // Return the penetration depth
#ifdef VISUAL_DEBUG
- void draw() const; // Draw the contact (for debugging)
+ void draw() const; // Draw the contact (for debugging)
#endif
};
@@ -96,14 +103,39 @@ inline Vector3 Contact::getNormal() const {
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
-inline Vector3 Contact::getPointOnBody1() const {
- return pointOnBody1;
+inline Vector3 Contact::getLocalPointOnBody1() const {
+ return localPointOnBody1;
}
// Return the contact point on body 2
-inline Vector3 Contact::getPointOnBody2() const {
- return pointOnBody2;
+inline Vector3 Contact::getLocalPointOnBody2() const {
+ 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
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 53d3a40c..78ae40f4 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -357,8 +357,8 @@ void ConstraintSolver::updateContactCache() {
// Get all the contact points of the contact
vector<Vector3> points;
vector<double> lambdas;
- points.push_back(contact->getPointOnBody1());
- points.push_back(contact->getPointOnBody2());
+ points.push_back(contact->getWorldPointOnBody1());
+ points.push_back(contact->getWorldPointOnBody2());
// For each constraint of the contact
for (int i=0; i<contact->getNbConstraints(); i++) {
diff --git a/src/engine/ContactCache.cpp b/src/engine/ContactCache.cpp
index 31c35851..2131c3ed 100644
--- a/src/engine/ContactCache.cpp
+++ b/src/engine/ContactCache.cpp
@@ -66,9 +66,9 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
assert((*entry).first.second == contact->getBody2());
// Get the position of the current contact
- posX = contact->getPointOnBody1().getX();
- posY = contact->getPointOnBody1().getY();
- posZ = contact->getPointOnBody1().getZ();
+ posX = contact->getWorldPointOnBody1().getX();
+ posY = contact->getWorldPointOnBody1().getY();
+ posZ = contact->getWorldPointOnBody1().getZ();
// Get the position of the old contact
Vector3& contactPos1 = contactInfo->positions[0];
@@ -83,9 +83,9 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
}
// Get the position of the current contact
- posX = contact->getPointOnBody2().getX();
- posY = contact->getPointOnBody2().getY();
- posZ = contact->getPointOnBody2().getZ();
+ posX = contact->getWorldPointOnBody2().getX();
+ posY = contact->getWorldPointOnBody2().getY();
+ posZ = contact->getWorldPointOnBody2().getZ();
// Get the position of the old contact
Vector3& contactPos2 = contactInfo->positions[1];
diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp
new file mode 100644
index 00000000..64bdce1c
--- /dev/null
+++ b/src/engine/PersistentContactCache.cpp
@@ -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;
+}
\ No newline at end of file
diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h
new file mode 100644
index 00000000..8c2763a9
--- /dev/null
+++ b/src/engine/PersistentContactCache.h
@@ -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
+
diff --git a/src/memory/MemoryPool.cpp b/src/memory/MemoryPool.cpp
new file mode 100644
index 00000000..f2a97c9d
--- /dev/null
+++ b/src/memory/MemoryPool.cpp
@@ -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;
+}
diff --git a/src/memory/MemoryPool.h b/src/memory/MemoryPool.h
new file mode 100644
index 00000000..1fc6564a
--- /dev/null
+++ b/src/memory/MemoryPool.h
@@ -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
+