diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 0dcb4e48..33779795 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include "GJKAlgorithm.h" #include "Simplex.h" -#include "../../../constraint/Contact.h" +#include "../../../constraint/ContactPoint.h" #include "../../../configuration.h" #include #include diff --git a/src/configuration.h b/src/configuration.h index 935ef6ff..e570f2d5 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -79,15 +79,11 @@ const decimal OBJECT_MARGIN = decimal(0.04); // Distance threshold for two contact points for a valid persistent contact (in meters) const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); +const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); + // Number of iterations when solving a LCP problem const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15; -// Number of iterations when solving a LCP problem for error correction -const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; - -// True if the error correction projection (first order world) is active in the constraint solver -const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; - } #endif diff --git a/src/constraint/Contact.cpp b/src/constraint/ContactPoint.cpp similarity index 74% rename from src/constraint/Contact.cpp rename to src/constraint/ContactPoint.cpp index 61403dfc..b5b87dcf 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/ContactPoint.cpp @@ -24,25 +24,27 @@ ********************************************************************************/ // Libraries -#include "Contact.h" +#include "ContactPoint.h" using namespace reactphysics3d; using namespace std; // Constructor -Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo) - : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), - mPenetrationDepth(contactInfo->penetrationDepth), - mLocalPointOnBody1(contactInfo->localPoint1), - mLocalPointOnBody2(contactInfo->localPoint2), - mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), - mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { +ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2, + const ContactInfo* contactInfo) + : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), + mPenetrationDepth(contactInfo->penetrationDepth), + mLocalPointOnBody1(contactInfo->localPoint1), + mLocalPointOnBody2(contactInfo->localPoint2), + mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), + mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), + mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { + assert(mPenetrationDepth > 0.0); } // Destructor -Contact::~Contact() { +ContactPoint::~ContactPoint() { } diff --git a/src/constraint/Contact.h b/src/constraint/ContactPoint.h similarity index 82% rename from src/constraint/Contact.h rename to src/constraint/ContactPoint.h index d18ef023..38379d2c 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/ContactPoint.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef CONTACT_H -#define CONTACT_H +#ifndef CONTACT_POINT_H +#define CONTACT_POINT_H // Libraries #include "Constraint.h" @@ -52,13 +52,13 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- - Class Contact : + Class ContactPoint : This class represents a collision contact point between two - bodies in the physics engine. The contact class inherits from + bodies in the physics engine. The ContactPoint class inherits from the Constraint class. ------------------------------------------------------------------- */ -class Contact : public Constraint { +class ContactPoint : public Constraint { protected : @@ -91,20 +91,20 @@ class Contact : public Constraint { // -------------------- Methods -------------------- // // Private copy-constructor - Contact(const Contact& contact); + ContactPoint(const ContactPoint& contact); // Private assignment operator - Contact& operator=(const Contact& contact); + ContactPoint& operator=(const ContactPoint& contact); public : // -------------------- Methods -------------------- // // Constructor - Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); + ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); // Destructor - virtual ~Contact(); + virtual ~ContactPoint(); // Return the normal vector of the contact Vector3 getNormal() const; @@ -158,83 +158,83 @@ class Contact : public Constraint { }; // Return the normal vector of the contact -inline Vector3 Contact::getNormal() const { +inline Vector3 ContactPoint::getNormal() const { return mNormal; } // Set the penetration depth of the contact -inline void Contact::setPenetrationDepth(decimal penetrationDepth) { +inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { this->mPenetrationDepth = penetrationDepth; } // Return the contact point on body 1 -inline Vector3 Contact::getLocalPointOnBody1() const { +inline Vector3 ContactPoint::getLocalPointOnBody1() const { return mLocalPointOnBody1; } // Return the contact point on body 2 -inline Vector3 Contact::getLocalPointOnBody2() const { +inline Vector3 ContactPoint::getLocalPointOnBody2() const { return mLocalPointOnBody2; } // Return the contact world point on body 1 -inline Vector3 Contact::getWorldPointOnBody1() const { +inline Vector3 ContactPoint::getWorldPointOnBody1() const { return mWorldPointOnBody1; } // Return the contact world point on body 2 -inline Vector3 Contact::getWorldPointOnBody2() const { +inline Vector3 ContactPoint::getWorldPointOnBody2() const { return mWorldPointOnBody2; } // Set the contact world point on body 1 -inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 -inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { mWorldPointOnBody2 = worldPoint; } // Return true if the contact is a resting contact -inline bool Contact::getIsRestingContact() const { +inline bool ContactPoint::getIsRestingContact() const { return mIsRestingContact; } // Set the mIsRestingContact variable -inline void Contact::setIsRestingContact(bool isRestingContact) { +inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } // Get the first friction vector -inline Vector3 Contact::getFrictionVector1() const { +inline Vector3 ContactPoint::getFrictionVector1() const { return mFrictionVectors[0]; } // Set the first friction vector -inline void Contact::setFrictionVector1(const Vector3& frictionVector1) { +inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { mFrictionVectors[0] = frictionVector1; } // Get the second friction vector -inline Vector3 Contact::getFrictionVector2() const { +inline Vector3 ContactPoint::getFrictionVector2() const { return mFrictionVectors[1]; } // Set the second friction vector -inline void Contact::setFrictionVector2(const Vector3& frictionVector2) { +inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { mFrictionVectors[1] = frictionVector2; } // Return the penetration depth of the contact -inline decimal Contact::getPenetrationDepth() const { +inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; } #ifdef VISUAL_DEBUG -inline void Contact::draw() const { +inline void ContactPoint::draw() const { glColor3f(1.0, 0.0, 0.0); glutSolidSphere(0.3, 20, 20); } diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 697103a4..52203b11 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { collisionBody->CollisionBody::~CollisionBody(); // Remove the collision body from the list of bodies - mBodies.erase(collisionBody); // TOOD : Maybe use a set to make this faster + mBodies.erase(collisionBody); // TODO : Maybe use a set to make this faster // Free the object from the memory pool mMemoryPoolCollisionBodies.freeObject(collisionBody); diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 5d9d01d9..8d105882 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -35,7 +35,7 @@ #include "OverlappingPair.h" #include "../collision/CollisionDetection.h" #include "../constraint/Constraint.h" -#include "../constraint/Contact.h" +#include "../constraint/ContactPoint.h" #include "../memory/MemoryPool.h" // Namespace reactphysics3d diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/ContactManifold.cpp similarity index 55% rename from src/engine/PersistentContactCache.cpp rename to src/engine/ContactManifold.cpp index 49b7490d..389a59d6 100644 --- a/src/engine/PersistentContactCache.cpp +++ b/src/engine/ContactManifold.cpp @@ -24,34 +24,37 @@ ********************************************************************************/ // Libraries -#include "PersistentContactCache.h" +#include "ContactManifold.h" using namespace reactphysics3d; // Constructor -PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) { +ContactManifold::ContactManifold(Body* const body1, Body* const body2, + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0), + mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), + mMemoryPoolContacts(memoryPoolContacts) { } // Destructor -PersistentContactCache::~PersistentContactCache() { +ContactManifold::~ContactManifold() { clear(); } -// Add a contact in the cache -void PersistentContactCache::addContact(Contact* contact) { +// Add a contact point in the manifold +void ContactManifold::addContactPoint(ContactPoint* contact) { - // For contact already in the cache - for (uint i=0; igetWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); + // already in the manifold. + decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { // Delete the new contact - contact->Contact::~Contact(); + contact->ContactPoint::~ContactPoint(); mMemoryPoolContacts.freeObject(contact); //removeContact(i); @@ -60,93 +63,95 @@ void PersistentContactCache::addContact(Contact* contact) { } } - // If the contact cache is full - if (mNbContacts == MAX_CONTACTS_IN_CACHE) { + // If the contact manifold is full + if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContact(indexToRemove); + removeContactPoint(indexToRemove); } - // Add the new contact in the cache - mContacts[mNbContacts] = contact; - mNbContacts++; + // Add the new contact point in the manifold + mContactPoints[mNbContactPoints] = contact; + mNbContactPoints++; } -// Remove a contact from the cache -void PersistentContactCache::removeContact(int index) { - assert(index >= 0 && index < mNbContacts); - assert(mNbContacts > 0); +// Remove a contact point from the manifold +void ContactManifold::removeContactPoint(int index) { + assert(index >= 0 && index < mNbContactPoints); + assert(mNbContactPoints > 0); // Call the destructor explicitly and tell the memory pool that // the corresponding memory block is now free - mContacts[index]->Contact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[index]); + mContactPoints[index]->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[index]); // If we don't remove the last index - if (index < mNbContacts - 1) { - mContacts[index] = mContacts[mNbContacts - 1]; + if (index < mNbContactPoints - 1) { + mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; } - mNbContacts--; + mNbContactPoints--; } -// Update the contact cache -// First the world space coordinates of the current contacts in the cache are recomputed from +// Update the contact manifold +// First the world space coordinates of the current contacts in the manifold 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) { - if (mNbContacts == 0) return; +void ContactManifold::update(const Transform& transform1, const Transform& transform2) { + if (mNbContactPoints == 0) return; - // Update the world coordinates and penetration depth of the contacts in the cache - for (int i=0; isetWorldPointOnBody1(transform1 * mContacts[i]->getLocalPointOnBody1()); - mContacts[i]->setWorldPointOnBody2(transform2 * mContacts[i]->getLocalPointOnBody2()); - mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); + // Update the world coordinates and penetration depth of the contact points in the manifold + for (int i=0; isetWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1()); + mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2()); + mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); } const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD; - // Remove the contacts that don't represent very well the persistent contact - for (int i=mNbContacts-1; i>=0; i--) { - assert(i>= 0 && i < mNbContacts); + // Remove the contact points that don't represent very well the contact manifold + for (int i=mNbContactPoints-1; i>=0; i--) { + assert(i>= 0 && i < mNbContactPoints); // Compute the distance between contact points in the normal direction - decimal distanceNormal = -mContacts[i]->getPenetrationDepth(); + decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); // If the contacts points are too far from each other in the normal direction if (distanceNormal > squarePersistentContactThreshold) { - removeContact(i); + removeContactPoint(i); } else { - // Compute the distance of the two contact points in the plane orthogonal to the contact normal - Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() + - mContacts[i]->getNormal() * distanceNormal; - Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; + // Compute the distance of the two contact points in the plane + // orthogonal to the contact normal + Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + + mContactPoints[i]->getNormal() * distanceNormal; + Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - // If the orthogonal distance is larger than the valid distance threshold, we remove the contact + // If the orthogonal distance is larger than the valid distance + // threshold, we remove the contact if (projDifference.lengthSquare() > squarePersistentContactThreshold) { - removeContact(i); + removeContactPoint(i); } } } } -// Return the index of the contact with the larger penetration depth. This +// Return the index of the contact point 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(mNbContacts == MAX_CONTACTS_IN_CACHE); +int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); int indexMaxPenetrationDepth = -1; decimal maxPenetrationDepth = newContact->getPenetrationDepth(); // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContacts[i]->getPenetrationDepth(); + if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); indexMaxPenetrationDepth = i; } } @@ -155,12 +160,18 @@ int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) co return indexMaxPenetrationDepth; } -// Return the index that will be removed. The index of the contact with the larger penetration +// Return the index that will be removed. The index of the contact point 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(mNbContacts == MAX_CONTACTS_IN_CACHE); +// kept. In order to compute the area of a quadrilateral, we use the formula : +// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that +// when we compute this area, we do not calculate it exactly but we +// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, +// this is only a guess that is faster to compute. +int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { + + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint @@ -168,29 +179,29 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect if (indexMaxPenetration != 0) { // Compute the area - Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area0 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 1) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area1 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 2) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area2 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 3) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[2]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area3 = crossProduct.lengthSquare(); } @@ -200,7 +211,7 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect } // Return the index of maximum area -int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { +int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { if (area0 < area1) { if (area1 < area2) { if (area2 < area3) return 3; @@ -223,14 +234,14 @@ int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal are } } -// Clear the cache -void PersistentContactCache::clear() { - for (uint i=0; iContact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[i]); + mContactPoints[i]->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[i]); } - mNbContacts = 0; + mNbContactPoints = 0; } diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h index 7e8a7816..33dff5d8 100644 --- a/src/engine/ContactManifold.h +++ b/src/engine/ContactManifold.h @@ -24,48 +24,221 @@ ********************************************************************************/ #ifndef CONTACT_MANIFOLD_H -#define CONTACT_MANIFOLD_H +#define CONTACT_MANIFOLD_H // Libraries -#include "../constraint/Contact.h" -#include "../configuration.h" +#include +#include "../body/Body.h" +#include "../constraint/ContactPoint.h" +// ReactPhysics3D namespace namespace reactphysics3d { -// Class ContactManifold -// This class contains several contact points between two bodies -struct ContactManifold { +// Constants +const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold + +/* ------------------------------------------------------------------- + Class ContactManifold : + This class represents the set of contact points between two bodies. + The contact manifold is implemented in a way to cache the contact + points among the frames for better stability following the + "Contact Generation" presentation of Erwin Coumans at GDC 2010 + conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). + Some code of this class is based on the implementation of the + btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). + 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. + ------------------------------------------------------------------- +*/ +class ContactManifold { + + private: // -------------------- Attributes -------------------- // - // Contact for each contact point - Contact* contacts[4]; // TODO : Use a constant here for the nb of contacts + // Pointer to the first body + Body* const mBody1; - // Number of contacts in the manifold - uint nbContacts; + // Pointer to the second body + Body* const mBody2; + + // Contact points in the manifold + ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + + // Number of contacts in the cache + uint mNbContactPoints; // First friction vector of the contact manifold - Vector3 frictionVector1; + Vector3 mFrictionVector1; // Second friction vector of the contact manifold - Vector3 frictionVector2; + Vector3 mFrictionVector2; // First friction constraint accumulated impulse - decimal friction1Impulse; + decimal mFrictionImpulse1; // Second friction constraint accumulated impulse - decimal friction2Impulse; + decimal mFrictionImpulse2; // Twist friction constraint accumulated impulse - decimal frictionTwistImpulse; + decimal mFrictionTwistImpulse; + + // Reference to the memory pool with the contacts + MemoryPool& mMemoryPoolContacts; + + // -------------------- Methods -------------------- // + + // Private copy-constructor + ContactManifold(const ContactManifold& contactManifold); + + // Private assignment operator + ContactManifold& operator=(const ContactManifold& contactManifold); + + // Return the index of maximum area + int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; + + // Return the index of the contact with the larger penetration depth + int getIndexOfDeepestPenetration(ContactPoint* newContact) const; + + // Return the index that will be removed + int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; + + // Remove a contact point from the manifold + void removeContactPoint(int index); + + // Return true if two vectors are approximatively equal + bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; + + public: // -------------------- Methods -------------------- // // Constructor - ContactManifold() : nbContacts(0), friction1Impulse(0.0), friction2Impulse(0.0), - frictionTwistImpulse(0.0) {} + ContactManifold(Body* const mBody1, Body* const mBody2, + MemoryPool& mMemoryPoolContacts); + + // Destructor + ~ContactManifold(); + + // Add a contact point to the manifold + void addContactPoint(ContactPoint* contact); + + // Update the contact manifold + void update(const Transform& transform1, const Transform& transform2); + + // Clear the contact manifold + void clear(); + + // Return the number of contact points in the manifold + uint getNbContactPoints() const; + + // Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + // set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + // Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + // set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + // Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + // Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + // Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + // Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + // Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; + + // Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + // Return a contact point of the manifold + ContactPoint* getContactPoint(uint index) const; }; +// Return the number of contact points in the manifold +inline uint ContactManifold::getNbContactPoints() const { + return mNbContactPoints; } +// Return the first friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector1() const { + return mFrictionVector1; +} + +// set the first friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { + mFrictionVector1 = frictionVector1; +} + +// Return the second friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector2() const { + return mFrictionVector2; +} + +// set the second friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { + mFrictionVector2 = frictionVector2; +} + +// Return the first friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse1() const { + return mFrictionImpulse1; +} + +// Set the first friction accumulated impulse +inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { + mFrictionImpulse1 = frictionImpulse1; +} + +// Return the second friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse2() const { + return mFrictionImpulse2; +} + +// Set the second friction accumulated impulse +inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { + mFrictionImpulse2 = frictionImpulse2; +} + +// Return the friction twist accumulated impulse +inline decimal ContactManifold::getFrictionTwistImpulse() const { + return mFrictionTwistImpulse; +} + +// Set the friction twist accumulated impulse +inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { + mFrictionTwistImpulse = frictionTwistImpulse; +} + +// Return a contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoint(uint index) const { + assert(index >= 0 && index < mNbContactPoints); + return mContactPoints[index]; +} + +// Return true if two vectors are approximatively equal +inline bool ContactManifold::isApproxEqual(const Vector3& vector1, + const Vector3& vector2) const { + const decimal epsilon = decimal(0.1); + return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && + approxEqual(vector1.getY(), vector2.getY(), epsilon) && + approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); +} + +} #endif + diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index e10bc29b..b0dfa4ac 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -37,16 +37,16 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(DynamicsWorld& world, std::vector& constrainedLinearVelocities, +ContactSolver::ContactSolver(DynamicsWorld& world,std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex) :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), - mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), + mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), + mContactConstraints(NULL), mConstrainedLinearVelocities(constrainedLinearVelocities), mConstrainedAngularVelocities(constrainedAngularVelocities), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), - mIsSplitImpulseActive(true), + mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -62,22 +62,22 @@ void ContactSolver::initialize() { // TODO : Use better memory allocation here mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()]; - mNbContactConstraints = 0; + mNbContactManifolds = 0; // For each contact manifold of the world - vector::iterator it; + vector::iterator it; for (it = mWorld.getContactManifoldsBeginIterator(); it != mWorld.getContactManifoldsEndIterator(); ++it) { - ContactManifold& externalContactManifold = *it; + ContactManifold* externalManifold = *it; - ContactManifoldSolver& internalContactManifold = mContactConstraints[mNbContactConstraints]; + ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds]; - assert(externalContactManifold.nbContacts > 0); + assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact - RigidBody* body1 = externalContactManifold.contacts[0]->getBody1(); - RigidBody* body2 = externalContactManifold.contacts[0]->getBody2(); + RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1(); + RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); // Add the two bodies of the constraint in the constraintBodies list mConstraintBodies.insert(body1); @@ -87,38 +87,40 @@ void ContactSolver::initialize() { Vector3 x1 = body1->getTransform().getPosition(); Vector3 x2 = body2->getTransform().getPosition(); - internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalContactManifold.isBody1Moving = body1->getIsMotionEnabled(); - internalContactManifold.isBody2Moving = body2->getIsMotionEnabled(); - internalContactManifold.massInverseBody1 = body1->getMassInverse(); - internalContactManifold.massInverseBody2 = body2->getMassInverse(); - internalContactManifold.nbContacts = externalContactManifold.nbContacts; - internalContactManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalContactManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalContactManifold.contactManifold = &(*it); + // Initialize the internal contact manifold structure using the external + // contact manifold + internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + internalManifold.isBody1Moving = body1->getIsMotionEnabled(); + internalManifold.isBody2Moving = body2->getIsMotionEnabled(); + internalManifold.massInverseBody1 = body1->getMassInverse(); + internalManifold.massInverseBody2 = body2->getMassInverse(); + internalManifold.nbContacts = externalManifold->getNbContactPoints(); + internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + internalManifold.externalContactManifold = externalManifold; // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); - internalContactManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); } - // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalContactManifold.contacts[c]; + ContactPointSolver& contactPoint = internalManifold.contacts[c]; // Get a contact point - Contact* externalContact = externalContactManifold.contacts[c]; + ContactPoint* externalContact = externalManifold->getContactPoint(c); // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.contact = externalContact; + contactPoint.externalContact = externalContact; contactPoint.normal = externalContact->getNormal(); contactPoint.r1 = p1 - x1; contactPoint.r2 = p2 - x2; @@ -133,39 +135,39 @@ void ContactSolver::initialize() { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 += p1; - internalContactManifold.frictionPointBody2 += p2; + internalManifold.frictionPointBody1 += p1; + internalManifold.frictionPointBody2 += p2; } } // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 /= static_cast(internalContactManifold.nbContacts); - internalContactManifold.frictionPointBody2 /= static_cast(internalContactManifold.nbContacts); - internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1; - internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2; - internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1; - internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2; + internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); + internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); + internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; + internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; + internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); + internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); // If warm starting is active if (mIsWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses - internalContactManifold.friction1Impulse = externalContactManifold.friction1Impulse; - internalContactManifold.friction2Impulse = externalContactManifold.friction2Impulse; - internalContactManifold.frictionTwistImpulse = externalContactManifold.frictionTwistImpulse; + internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); } else { // Initialize the accumulated impulses to zero - internalContactManifold.friction1Impulse = 0.0; - internalContactManifold.friction2Impulse = 0.0; - internalContactManifold.frictionTwistImpulse = 0.0; + internalManifold.friction1Impulse = 0.0; + internalManifold.friction2Impulse = 0.0; + internalManifold.frictionTwistImpulse = 0.0; } } - mNbContactConstraints++; + mNbContactManifolds++; } // Allocated memory for split impulse velocities @@ -179,10 +181,13 @@ void ContactSolver::initialize() { assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size()); assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size()); + + // Initialize the split impulse velocities + initializeSplitImpulseVelocities(); } -// Initialize the constrained bodies -void ContactSolver::initializeBodies() { +// Initialize the split impulse velocities +void ContactSolver::initializeSplitImpulseVelocities() { // For each current body that is implied in some constraint set::iterator it; @@ -192,6 +197,7 @@ void ContactSolver::initializeBodies() { uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; + // Initialize the split impulse velocities to zero mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); } @@ -201,30 +207,30 @@ void ContactSolver::initializeBodies() { void ContactSolver::initializeContactConstraints() { // For each contact constraint - for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0); + massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / + massPenetration : + decimal(0.0); + // If we do not solve the friction constraints at the center of the contact manifold if (!mIsSolveFrictionAtContactManifoldCenterActive) { // Compute the friction vectors @@ -253,34 +263,48 @@ void ContactSolver::initializeContactConstraints() { contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); - // Compute the inverse mass matrix K for the friction constraints at each contact point + // Compute the inverse mass matrix K for the friction + // constraints at each contact point decimal friction1Mass = 0.0; decimal friction2Mass = 0.0; - if (contactManifold.isBody1Moving) { - friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1); - friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionVector2); + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( + contactPoint.frictionVector2); } - if (contactManifold.isBody2Moving) { - friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); - friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionVector2); + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( + contactPoint.frictionVector2); } - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / + friction1Mass : + decimal(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / + friction2Mass : + decimal(0.0); } // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal velocity - // under a given threshold), we don't add a restitution velocity bias + // at the beginning of the contact. Note that if it is a resting contact (normal + // velocity under a given threshold), we don't add a restitution velocity bias contactPoint.restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(contactPoint.normal); - // TODO : Use a constant here - if (deltaVDotN < 1.0f) { - contactPoint.restitutionBias = contactManifold.restitutionFactor * deltaVDotN; + if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) { + contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } - // Get the cached lambda values of the constraint + // If the warm starting of the contact solver is active if (mIsWarmStartingActive) { + + // Get the cached accumulated impulses from the previous step contactPoint.penetrationImpulse = externalContact->getCachedLambda(0); contactPoint.friction1Impulse = externalContact->getCachedLambda(1); contactPoint.friction2Impulse = externalContact->getCachedLambda(2); @@ -291,39 +315,58 @@ void ContactSolver::initializeContactConstraints() { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - contactManifold.normal += contactPoint.normal; + manifold.normal += contactPoint.normal; } } // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - contactManifold.normal.normalize(); + manifold.normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, contactManifold); + computeFrictionVectors(deltaVFrictionPoint, manifold); // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold - contactManifold.r1CrossT1 = contactManifold.r1Friction.cross(contactManifold.frictionVector1); - contactManifold.r1CrossT2 = contactManifold.r1Friction.cross(contactManifold.frictionVector2); - contactManifold.r2CrossT1 = contactManifold.r2Friction.cross(contactManifold.frictionVector1); - contactManifold.r2CrossT2 = contactManifold.r2Friction.cross(contactManifold.frictionVector2); + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); decimal friction1Mass = 0.0; decimal friction2Mass = 0.0; - if (contactManifold.isBody1Moving) { - friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT1).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector1); - friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT2).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector2); + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( + manifold.frictionVector2); } - if (contactManifold.isBody2Moving) { - friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT1).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector1); - friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT2).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector2); + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( + manifold.frictionVector2); } - friction1Mass > 0.0 ? contactManifold.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); - friction2Mass > 0.0 ? contactManifold.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + decimal frictionTwistMass = manifold.normal.dot( + manifold.inverseInertiaTensorBody1 * + manifold.normal) + + manifold.normal.dot( + manifold.inverseInertiaTensorBody2 * + manifold.normal); + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass + : decimal(0.0); + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass + : decimal(0.0); + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / + frictionTwistMass : + decimal(0.0); } } } @@ -335,7 +378,7 @@ void ContactSolver::initializeContactConstraints() { void ContactSolver::warmStart() { // For each constraint - for (uint c=0; csetCachedLambda(0, contactPoint.penetrationImpulse); - contactPoint.contact->setCachedLambda(1, contactPoint.friction1Impulse); - contactPoint.contact->setCachedLambda(2, contactPoint.friction2Impulse); + contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse); + contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse); + contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse); - contactPoint.contact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.contact->setFrictionVector2(contactPoint.frictionVector2); + contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); + contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); } - contactManifold.contactManifold->friction1Impulse = contactManifold.friction1Impulse; - contactManifold.contactManifold->friction2Impulse = contactManifold.friction2Impulse; - contactManifold.contactManifold->frictionTwistImpulse = contactManifold.frictionTwistImpulse; - contactManifold.contactManifold->frictionVector1 = contactManifold.frictionVector1; - contactManifold.contactManifold->frictionVector2 = contactManifold.frictionVector2; + manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); + manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); } } // Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold) { +void ContactSolver::applyImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { // Update the velocities of the bodies by applying the impulse P - if (contactManifold.isBody1Moving) { - mConstrainedLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + if (manifold.isBody1Moving) { + mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; - mConstrainedAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; } - if (contactManifold.isBody2Moving) { - mConstrainedLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * + if (manifold.isBody2Moving) { + mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2; - mConstrainedAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } } // Apply an impulse to the two bodies of a constraint void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& contactManifold) { + const ContactManifoldSolver& manifold) { // Update the velocities of the bodies by applying the impulse P - if (contactManifold.isBody1Moving) { - mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + if (manifold.isBody1Moving) { + mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; - mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; } - if (contactManifold.isBody2Moving) { - mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + if (manifold.isBody2Moving) { + mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } } @@ -781,7 +862,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contactPoint.frictionVector2 = contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); + contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane @@ -816,7 +897,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Clean up the constraint solver void ContactSolver::cleanup() { - //mMapBodyToIndex.clear(); + mConstraintBodies.clear(); if (mContactConstraints != NULL) { diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index bae7a9bb..1eb7c059 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -23,14 +23,14 @@ * * ********************************************************************************/ -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H +#ifndef CONTACT_SOLVER_H +#define CONTACT_SOLVER_H // Libraries -#include "../constraint/Contact.h" -#include "ContactManifold.h" +#include "../constraint/ContactPoint.h" #include "../configuration.h" #include "../constraint/Constraint.h" +#include "ContactManifold.h" #include #include @@ -124,10 +124,10 @@ struct Impulse { There are two ways to apply the friction constraints. Either the friction constraints are applied at each contact point or they are applied only at the center of the contact manifold between two bodies. If we solve the friction constraints at each contact point, we need - two constraints (two tangential friction directions) and if we solve the friction constraints - at the center of the contact manifold, we need two constraints for tangential friction but - also another twist friction constraint to prevent spin of the body around the contact - manifold center. + two constraints (two tangential friction directions) and if we solve the friction + constraints at the center of the contact manifold, we need two constraints for tangential + friction but also another twist friction constraint to prevent spin of the body around the + contact manifold center. ------------------------------------------------------------------- */ @@ -135,7 +135,7 @@ class ContactSolver { private: - // Structure ContactPoint + // Structure ContactPointSolver // Contact solver internal data structure that to store all the // information relative to a contact point struct ContactPointSolver { @@ -163,16 +163,14 @@ class ContactSolver { decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction bool isRestingContact; // True if the contact was existing last time step - Contact* contact; // Pointer to the external contact + ContactPoint* externalContact; // Pointer to the external contact }; - // Structure ContactManifold + // Structure ContactManifoldSolver // Contact solver internal data structure to store all the // information relative to a contact manifold struct ContactManifoldSolver { - // TODO : Use a constant for the number of contact points - uint indexBody1; // Index of body 1 in the constraint solver uint indexBody2; // Index of body 2 in the constraint solver decimal massInverseBody1; // Inverse of the mass of body 1 @@ -181,11 +179,11 @@ class ContactSolver { Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 bool isBody1Moving; // True if the body 1 is allowed to move bool isBody2Moving; // True if the body 2 is allowed to move - ContactPointSolver contacts[4]; // Contact point constraints + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];// Contact point constraints uint nbContacts; // Number of contact points decimal restitutionFactor; // Mix of the restitution factor for two bodies decimal frictionCoefficient; // Mix friction coefficient for the two bodies - ContactManifold* contactManifold; // Pointer to the external contact manifold + ContactManifold* externalContactManifold; // Pointer to the external contact manifold // Variables used when friction constraints are apply at the center of the manifold // @@ -200,6 +198,7 @@ class ContactSolver { Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector decimal inverseFriction1Mass; // Matrix K for the first friction constraint decimal inverseFriction2Mass; // Matrix K for the second friction constraint + decimal inverseTwistFrictionMass; // Matrix K for the twist friction constraint Vector3 frictionVector1; // First friction direction at contact manifold center Vector3 frictionVector2; // Second friction direction at contact manifold center Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center @@ -241,7 +240,7 @@ class ContactSolver { ContactManifoldSolver* mContactConstraints; // Number of contact constraints - uint mNbContactConstraints; + uint mNbContactManifolds; // Constrained bodies std::set mConstraintBodies; @@ -272,8 +271,8 @@ class ContactSolver { // Initialize the constraint solver void initialize(); - // Initialize the constrained bodies - void initializeBodies(); + // Initialize the split impulse velocities + void initializeSplitImpulseVelocities(); // Initialize the contact constraints before solving the system void initializeContactConstraints(); @@ -289,16 +288,19 @@ class ContactSolver { void solveContactConstraints(); // Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold); + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); // Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold); + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); // Compute the collision restitution factor from the restitution factor of each body - decimal computeMixedRestitutionFactor(const RigidBody* body1, const RigidBody* body2) const; + decimal computeMixedRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const; // Compute the mixed friction coefficient from the friction coefficient of each body - decimal computeMixedFrictionCoefficient(const RigidBody* body1, const RigidBody* body2)const; + decimal computeMixedFrictionCoefficient(const RigidBody* body1, + const RigidBody* body2)const; // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction // plane for a contact point constraint. The two vectors have to be @@ -330,8 +332,8 @@ class ContactSolver { // Constructor ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, const std::map& - mapBodyToVelocityIndex); + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex); // Destructor virtual ~ContactSolver(); @@ -432,16 +434,20 @@ inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambd inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, const ContactPointSolver& contactPoint) const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, contactPoint.r2CrossT1 * deltaLambda); + return Impulse(-contactPoint.frictionVector1 * deltaLambda, + -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, + contactPoint.r2CrossT1 * deltaLambda); } // Compute the second friction constraint impulse inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, const ContactPointSolver& contactPoint) const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); + return Impulse(-contactPoint.frictionVector2 * deltaLambda, + -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionVector2 * deltaLambda, + contactPoint.r2CrossT2 * deltaLambda); } } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index d1ad2f96..a24147f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -305,7 +305,7 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const assert(rigidBody2); // Create a new contact - Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo); + ContactPoint* contact = new (mMemoryPoolContacts.allocateObject()) ContactPoint(rigidBody1, rigidBody2, contactInfo); assert(contact); // Get the corresponding overlapping pair @@ -316,6 +316,8 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add the contact to the contact cache of the corresponding overlapping pair overlappingPair->addContact(contact); + // TODO : Remove this + /* // Create a contact manifold with the contact points of the two bodies ContactManifold contactManifold; contactManifold.nbContacts = 0; @@ -326,7 +328,8 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const contactManifold.contacts[i] = overlappingPair->getContact(i); contactManifold.nbContacts++; } + */ // Add the contact manifold to the world - mContactManifolds.push_back(contactManifold); + mContactManifolds.push_back(overlappingPair->getContactManifold()); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b2254fca..7bddf900 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -28,7 +28,6 @@ // Libraries #include "CollisionWorld.h" -#include "ContactManifold.h" #include "../collision/CollisionDetection.h" #include "ContactSolver.h" #include "../body/RigidBody.h" @@ -64,7 +63,7 @@ class DynamicsWorld : public CollisionWorld { std::set mRigidBodies; // All the contact constraints - std::vector mContactManifolds; + std::vector mContactManifolds; // All the constraints (except contact constraints) std::vector mConstraints; @@ -82,7 +81,7 @@ class DynamicsWorld : public CollisionWorld { MemoryPool mMemoryPoolRigidBodies; // Memory pool for the contacts - MemoryPool mMemoryPoolContacts; + MemoryPool mMemoryPoolContacts; // Array of constrained linear velocities (state of the linear velocities // after solving the constraints) @@ -208,10 +207,10 @@ public : std::vector::iterator getConstraintsEndIterator(); // Return a start iterator on the contact manifolds list - std::vector::iterator getContactManifoldsBeginIterator(); + std::vector::iterator getContactManifoldsBeginIterator(); // Return a end iterator on the contact manifolds list - std::vector::iterator getContactManifoldsEndIterator(); + std::vector::iterator getContactManifoldsEndIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -337,12 +336,12 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat } // Return a start iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { +inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { return mContactManifolds.begin(); } // Return a end iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { +inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { return mContactManifolds.end(); } diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 933dac4e..cfd0aef4 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,11 +31,11 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts), + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts), mCachedSeparatingAxis(1.0, 1.0, 1.0) { -} +} // Destructor OverlappingPair::~OverlappingPair() { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index f8d31aff..d8d977eb 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -27,7 +27,7 @@ #define OVERLAPPING_PAIR_H // Libraries -#include "PersistentContactCache.h" +#include "ContactManifold.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -53,8 +53,8 @@ class OverlappingPair { // Pointer to the second body of the contact CollisionBody* const mBody2; - // Persistent contact cache - PersistentContactCache mContactsCache; + // Persistent contact manifold + ContactManifold mContactManifold; // Cached previous separating axis Vector3 mCachedSeparatingAxis; @@ -73,7 +73,7 @@ class OverlappingPair { // Constructor OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts); + MemoryPool& memoryPoolContacts); // Destructor ~OverlappingPair(); @@ -85,7 +85,7 @@ class OverlappingPair { CollisionBody* const getBody2() const; // Add a contact to the contact cache - void addContact(Contact* contact); + void addContact(ContactPoint* contact); // Update the contact cache void update(); @@ -99,8 +99,12 @@ class OverlappingPair { // Return the number of contacts in the cache uint getNbContacts() const; + // Return the contact manifold + ContactManifold* getContactManifold(); + // Return a contact of the cache - Contact* getContact(uint index) const; + // TODO : Maybe remove this method + ContactPoint* getContact(uint index) const; }; // Return the pointer to first body @@ -113,14 +117,14 @@ inline CollisionBody* const OverlappingPair::getBody2() const { return mBody2; } -// Add a contact to the contact cache -inline void OverlappingPair::addContact(Contact* contact) { - mContactsCache.addContact(contact); +// Add a contact to the contact manifold +inline void OverlappingPair::addContact(ContactPoint* contact) { + mContactManifold.addContactPoint(contact); } -// Update the contact cache +// Update the contact manifold inline void OverlappingPair::update() { - mContactsCache.update(mBody1->getTransform(), mBody2->getTransform()); + mContactManifold.update(mBody1->getTransform(), mBody2->getTransform()); } // Return the cached separating axis @@ -136,12 +140,17 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { // Return the number of contacts in the cache inline uint OverlappingPair::getNbContacts() const { - return mContactsCache.getNbContacts(); + return mContactManifold.getNbContactPoints(); +} + +// Return the contact manifold +inline ContactManifold* OverlappingPair::getContactManifold() { + return &mContactManifold; } // Return a contact of the cache -inline Contact* OverlappingPair::getContact(uint index) const { - return mContactsCache.getContact(index); +inline ContactPoint* OverlappingPair::getContact(uint index) const { + return mContactManifold.getContactPoint(index); } } // End of the ReactPhysics3D namespace diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h deleted file mode 100644 index a1b28a8d..00000000 --- a/src/engine/PersistentContactCache.h +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef PERSISTENT_CONTACT_CACHE_H -#define PERSISTENT_CONTACT_CACHE_H - -// Libraries -#include -#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: - - // -------------------- Attributes -------------------- // - - // Pointer to the first body - Body* const mBody1; - - // Pointer to the second body - Body* const mBody2; - - // Contacts in the cache - Contact* mContacts[MAX_CONTACTS_IN_CACHE]; - - // Number of contacts in the cache - uint mNbContacts; - - // Reference to the memory pool with the contacts - MemoryPool& mMemoryPoolContacts; - - // -------------------- Methods -------------------- // - - // Private copy-constructor - PersistentContactCache(const PersistentContactCache& persistentContactCache); - - // Private assignment operator - PersistentContactCache& operator=(const PersistentContactCache& persistentContactCache); - - // Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; - - // Return the index of the contact with the larger penetration depth - int getIndexOfDeepestPenetration(Contact* newContact) const; - - // Return the index that will be removed - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; - - // Remove a contact from the cache - void removeContact(int index); - - // Return true if two vectors are approximatively equal - bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; - - public: - - // -------------------- Methods -------------------- // - - // Constructor - PersistentContactCache(Body* const mBody1, Body* const mBody2, - MemoryPool& mMemoryPoolContacts); - - // Destructor - ~PersistentContactCache(); - - // Add a contact - void addContact(Contact* contact); - - // Update the contact cache - void update(const Transform& transform1, const Transform& transform2); - - // Clear the cache - void clear(); - - // Return the number of contacts in the cache - uint getNbContacts() const; - - // Return a contact of the cache - Contact* getContact(uint index) const; -}; - -// Return the number of contacts in the cache -inline uint PersistentContactCache::getNbContacts() const { - return mNbContacts; -} - -// Return a contact of the cache -inline Contact* PersistentContactCache::getContact(uint index) const { - assert(index >= 0 && index < mNbContacts); - return mContacts[index]; -} - -// Return true if two vectors are approximatively equal -inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, - const Vector3& vector2) const { - const decimal epsilon = decimal(0.1); - return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && - approxEqual(vector1.getY(), vector2.getY(), epsilon) && - approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); -} - -} -#endif -