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

Clean the code of the sequential impulse contact solver

Browse Source
This commit is contained in:
Daniel Chappuis 2013-02-26 22:43:45 +01:00
parent b36f2c93b1
commit a362171532
15 changed files with 661 additions and 528 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,7 +26,7 @@
// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "../../../constraint/Contact.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../configuration.h"
#include <algorithm>
#include <cmath>

8
src/configuration.h
View File

@ -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

22
src/constraint/Contact.cpp → src/constraint/ContactPoint.cpp
View File

@ -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() {
}

50
src/constraint/Contact.h → src/constraint/ContactPoint.h
View File

@ -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);
}

2
src/engine/CollisionWorld.cpp
View File

@ -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);

2
src/engine/CollisionWorld.h
View File

@ -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

153
src/engine/PersistentContactCache.cpp → src/engine/ContactManifold.cpp
View File

@ -24,34 +24,37 @@
********************************************************************************/
// Libraries
#include "PersistentContactCache.h"
#include "ContactManifold.h"
using namespace reactphysics3d;
// Constructor
PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
: mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) {
ContactManifold::ContactManifold(Body* const body1, Body* const body2,
MemoryPool<ContactPoint>& 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; i<mNbContacts; i++) {
// For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the cache.
decimal distance = (mContacts[i]->getWorldPointOnBody1() - 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; i<mNbContacts; i++) {
mContacts[i]->setWorldPointOnBody1(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; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(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; i<mNbContacts; i++) {
for (uint i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (mContacts[i]->getPenetrationDepth() > 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; i<mNbContacts; i++) {
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory pool that
// the corresponding memory block is now free
mContacts[i]->Contact::~Contact();
mMemoryPoolContacts.freeObject(mContacts[i]);
mContactPoints[i]->ContactPoint::~ContactPoint();
mMemoryPoolContacts.freeObject(mContactPoints[i]);
}
mNbContacts = 0;
mNbContactPoints = 0;
}

207
src/engine/ContactManifold.h
View File

@ -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 <vector>
#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<ContactPoint>& 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<ContactPoint>& 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

471
src/engine/ContactSolver.cpp
View File

@ -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<Vector3>& constrainedLinearVelocities,
ContactSolver::ContactSolver(DynamicsWorld& world,std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& 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<ContactManifold>::iterator it;
vector<ContactManifold*>::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; c<externalContactManifold.nbContacts; c++) {
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); 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<decimal>(internalContactManifold.nbContacts);
internalContactManifold.frictionPointBody2 /= static_cast<decimal>(internalContactManifold.nbContacts);
internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1;
internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2;
internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1;
internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2;
internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
internalManifold.frictionPointBody2 /=static_cast<decimal>(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<RigidBody*>::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<mNbContactConstraints; c++) {
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& manifold = mContactConstraints[c];
// Get the inertia tensors of both bodies
Matrix3x3& I1 = contactManifold.inverseInertiaTensorBody1;
Matrix3x3& I2 = contactManifold.inverseInertiaTensorBody2;
Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
contactManifold.normal = Vector3(0.0, 0.0, 0.0);
manifold.normal = Vector3(0.0, 0.0, 0.0);
}
// Get the velocities of the bodies
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
const Vector3& v1 = mConstrainedLinearVelocities[manifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[manifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[manifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[manifold.indexBody2];
// For each contact point constraint
for (uint i=0; i<contactManifold.nbContacts; i++) {
for (uint i=0; i<manifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
Contact* externalContact = contactPoint.contact;
ContactPointSolver& contactPoint = manifold.contacts[i];
ContactPoint* externalContact = contactPoint.externalContact;
// Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
@ -232,17 +238,21 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// Compute the inverse mass matrix K for the penetration constraint
decimal massPenetration = 0.0;
if (contactManifold.isBody1Moving) {
massPenetration += contactManifold.massInverseBody1 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal);
if (manifold.isBody1Moving) {
massPenetration += manifold.massInverseBody1 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal);
}
if (contactManifold.isBody2Moving) {
massPenetration += contactManifold.massInverseBody2 +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
if (manifold.isBody2Moving) {
massPenetration += manifold.massInverseBody2 +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
}
massPenetration > 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; c<mNbContactConstraints; c++) {
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
@ -352,9 +395,9 @@ void ContactSolver::warmStart() {
// --------- Penetration --------- //
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(contactPoint.penetrationImpulse,
contactPoint);
// Compute the impulse P = J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(
contactPoint.penetrationImpulse, contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
@ -362,18 +405,22 @@ void ContactSolver::warmStart() {
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse * contactPoint.oldFrictionVector1 +
contactPoint.friction2Impulse * contactPoint.oldFrictionVector2;
contactPoint.friction1Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector1);
contactPoint.friction2Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector2);
// Project the old friction impulses (with old friction vectors) into
// the new friction vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
contactPoint.oldFrictionVector1 +
contactPoint.friction2Impulse *
contactPoint.oldFrictionVector2;
contactPoint.friction1Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector1);
contactPoint.friction2Impulse = oldFrictionImpulse.dot(
contactPoint.frictionVector2);
// --------- Friction 1 --------- //
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(contactPoint.friction1Impulse,
contactPoint);
// Compute the impulse P = J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(
contactPoint.friction1Impulse, contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
@ -381,8 +428,8 @@ void ContactSolver::warmStart() {
// --------- Friction 2 --------- //
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(contactPoint.friction2Impulse,
contactPoint);
const Impulse impulseFriction2 = computeFriction2Impulse(
contactPoint.friction2Impulse, contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
@ -403,41 +450,52 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 +
contactManifold.friction2Impulse * contactManifold.oldFrictionVector2;
contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2);
Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
contactManifold.oldFrictionVector1 +
contactManifold.friction2Impulse *
contactManifold.oldFrictionVector2;
contactManifold.friction1Impulse = oldFrictionImpulse.dot(
contactManifold.frictionVector1);
contactManifold.friction2Impulse = oldFrictionImpulse.dot(
contactManifold.frictionVector2);
// ------ First friction constraint at the center of the contact manifol ------ //
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse;
// Compute the impulse P = J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
contactManifold.friction1Impulse;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
contactManifold.friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
contactManifold.friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
contactManifold.friction1Impulse;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * contactManifold.friction2Impulse;
angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse;
angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
// Compute the impulse P = J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 *
contactManifold.friction2Impulse;
angularImpulseBody1 = -contactManifold.r1CrossT2 *
contactManifold.friction2Impulse;
linearImpulseBody2 = contactManifold.frictionVector2 *
contactManifold.friction2Impulse;
angularImpulseBody2 = contactManifold.r2CrossT2 *
contactManifold.friction2Impulse;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P=J^T * lambda
// Compute the impulse P = J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
@ -465,16 +523,17 @@ void ContactSolver::solveContactConstraints() {
decimal lambdaTemp;
uint iter;
// For each iteration
// For each iteration of the contact solver
for(iter=0; iter<mNbIterations; iter++) {
// For each constraint
for (uint c=0; c<mNbContactConstraints; c++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
@ -498,19 +557,22 @@ void ContactSolver::solveContactConstraints() {
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f);
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint);
const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
contactPoint);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
@ -525,16 +587,20 @@ void ContactSolver::solveContactConstraints() {
const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1);
Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
v1Split - w1Split.cross(contactPoint.r1);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
contactPoint.penetrationSplitImpulse = std::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda
const Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit,
contactPoint);
const Impulse splitImpulsePenetration = computePenetrationImpulse(
deltaLambdaSplit, contactPoint);
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
@ -548,15 +614,20 @@ void ContactSolver::solveContactConstraints() {
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector1);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
decimal frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = std::max(-frictionLimit, std::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit));
contactPoint.friction1Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction1Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint);
const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
@ -567,15 +638,20 @@ void ContactSolver::solveContactConstraints() {
deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
Jv = deltaV.dot(contactPoint.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv;
deltaLambda *= contactPoint.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
frictionLimit = contactManifold.frictionCoefficient *
contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = std::max(-frictionLimit, std::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit));
contactPoint.friction2Impulse = std::max(-frictionLimit,
std::min(contactPoint.friction2Impulse
+ deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint);
const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
@ -588,13 +664,17 @@ void ContactSolver::solveContactConstraints() {
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction);
Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
decimal Jv = deltaV.dot(contactManifold.frictionVector1);
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction1Impulse;
contactManifold.friction1Impulse = std::max(-frictionLimit, std::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit));
contactManifold.friction1Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction1Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
@ -611,13 +691,17 @@ void ContactSolver::solveContactConstraints() {
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction);
deltaV = v2 + w2.cross(contactManifold.r2Friction)
- v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse;
contactManifold.friction2Impulse = std::max(-frictionLimit, std::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit));
contactManifold.friction2Impulse = std::max(-frictionLimit,
std::min(contactManifold.friction2Impulse +
deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
@ -633,19 +717,16 @@ void ContactSolver::solveContactConstraints() {
// ------ Twist friction constraint at the center of the contact manifol ------ //
// TODO : Put this in the initialization method
decimal K = contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody1 * contactManifold.normal) +
contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody2 * contactManifold.normal);
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
// TODO : Compute the inverse mass matrix here for twist friction
deltaLambda = -Jv * (decimal(1.0) / K);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit, std::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit));
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
@ -666,13 +747,12 @@ void ContactSolver::solveContactConstraints() {
// Solve the constraints
void ContactSolver::solve(decimal timeStep) {
// Set the current time step
mTimeStep = timeStep;
// Initialize the solver
initialize();
initializeBodies();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
@ -692,64 +772,65 @@ void ContactSolver::solve(decimal timeStep) {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
// For each constraint
for (uint c=0; c<mNbContactConstraints; c++) {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
ContactManifoldSolver& manifold = mContactConstraints[c];
for (uint i=0; i<contactManifold.nbContacts; i++) {
for (uint i=0; i<manifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
ContactPointSolver& contactPoint = manifold.contacts[i];
contactPoint.contact->setCachedLambda(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) {

62
src/engine/ContactSolver.h
View File

@ -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 <map>
#include <set>
@ -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<RigidBody*> 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<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities, const std::map<RigidBody*, uint>&
mapBodyToVelocityIndex);
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& 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);
}
}

7
src/engine/DynamicsWorld.cpp
View File

@ -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());
}

13
src/engine/DynamicsWorld.h
View File

@ -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<RigidBody*> mRigidBodies;
// All the contact constraints
std::vector<ContactManifold> mContactManifolds;
std::vector<ContactManifold*> mContactManifolds;
// All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints;
@ -82,7 +81,7 @@ class DynamicsWorld : public CollisionWorld {
MemoryPool<RigidBody> mMemoryPoolRigidBodies;
// Memory pool for the contacts
MemoryPool<Contact> mMemoryPoolContacts;
MemoryPool<ContactPoint> mMemoryPoolContacts;
// Array of constrained linear velocities (state of the linear velocities
// after solving the constraints)
@ -208,10 +207,10 @@ public :
std::vector<Constraint*>::iterator getConstraintsEndIterator();
// Return a start iterator on the contact manifolds list
std::vector<ContactManifold>::iterator getContactManifoldsBeginIterator();
std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
// Return a end iterator on the contact manifolds list
std::vector<ContactManifold>::iterator getContactManifoldsEndIterator();
std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -337,12 +336,12 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
}
// Return a start iterator on the contact manifolds list
inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsBeginIterator() {
return mContactManifolds.begin();
}
// Return a end iterator on the contact manifolds list
inline std::vector<ContactManifold>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
inline std::vector<ContactManifold*>::iterator DynamicsWorld::getContactManifoldsEndIterator() {
return mContactManifolds.end();
}

6
src/engine/OverlappingPair.cpp
View File

@ -31,11 +31,11 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
MemoryPool<Contact>& memoryPoolContacts)
: mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts),
MemoryPool<ContactPoint>& memoryPoolContacts)
: mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts),
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
}
}
// Destructor
OverlappingPair::~OverlappingPair() {

37
src/engine/OverlappingPair.h
View File

@ -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<Contact>& memoryPoolContacts);
MemoryPool<ContactPoint>& 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

147
src/engine/PersistentContactCache.h
View File

@ -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 <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:
// -------------------- 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<Contact>& 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<Contact>& 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