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

Merge branch 'sequentialimpulse2' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2013-02-26 22:45:01 +01:00
commit 33a084c816
33 changed files with 2305 additions and 1593 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/body/RigidBody.cpp
View File

@ -35,9 +35,9 @@ using namespace reactphysics3d;
CollisionShape *collisionShape, bodyindex id)
: CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
mMassInverse(1.0/mass) {
mMassInverse(decimal(1.0) / mass), mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT) {
mRestitution = 1.0;
mRestitution = decimal(1.0);
// Set the body pointer of the AABB and the collision shape
mAabb->setBodyPointer(this);

18
src/body/RigidBody.h
View File

@ -77,6 +77,8 @@ class RigidBody : public CollisionBody {
// Coefficient of restitution (between 0 and 1) where 1 is for a very bouncy body
decimal mRestitution;
// Friction coefficient
decimal mFrictionCoefficient;
// -------------------- Methods -------------------- //
@ -153,6 +155,12 @@ class RigidBody : public CollisionBody {
// Set the restitution coefficient
void setRestitution(decimal restitution) throw(std::invalid_argument);
// Get the friction coefficient
decimal getFrictionCoefficient() const;
// Set the friction coefficient
void setFrictionCoefficient(decimal frictionCoefficient);
};
// Method that return the mass of the body
@ -277,6 +285,16 @@ inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_ar
}
}
// Get the friction coefficient
inline decimal RigidBody::getFrictionCoefficient() const {
return mFrictionCoefficient;
}
// Set the friction coefficient
inline void RigidBody::setFrictionCoefficient(decimal frictionCoefficient) {
mFrictionCoefficient = frictionCoefficient;
}
} // End of the ReactPhyscis3D namespace
#endif

1
src/collision/narrowphase/EPA/EdgeEPA.h
View File

@ -105,6 +105,7 @@ inline int EdgeEPA::getIndex() const {
inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex;
return *this;
}
// Return the index of the next counter-clockwise edge of the ownver triangle

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>

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

@ -37,7 +37,7 @@
namespace reactphysics3d {
// Constants
const decimal REL_ERROR = 1.0e-3;
const decimal REL_ERROR = decimal(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
/* -------------------------------------------------------------------

2
src/collision/shapes/BoxShape.cpp
View File

@ -57,7 +57,7 @@ BoxShape::~BoxShape() {
// Return the local inertia tensor of the collision shape
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (1.0 / 3.0) * mass;
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
decimal xSquare = mExtent.getX() * mExtent.getX();
decimal ySquare = mExtent.getY() * mExtent.getY();
decimal zSquare = mExtent.getZ() * mExtent.getZ();

2
src/collision/shapes/ConeShape.cpp
View File

@ -45,7 +45,7 @@ using namespace reactphysics3d;
// Constructor
ConeShape::ConeShape(decimal radius, decimal height)
: CollisionShape(CONE), mRadius(radius), mHalfHeight(height/2.0) {
: CollisionShape(CONE), mRadius(radius), mHalfHeight(height / decimal(2.0)) {
assert(radius > 0.0);
assert(mHalfHeight > 0.0);

10
src/collision/shapes/ConeShape.h
View File

@ -118,12 +118,12 @@ inline void ConeShape::setRadius(decimal radius) {
// Return the height
inline decimal ConeShape::getHeight() const {
return 2.0 * mHalfHeight;
return decimal(2.0) * mHalfHeight;
}
// Set the height
inline void ConeShape::setHeight(decimal height) {
mHalfHeight = height / 2.0;
mHalfHeight = height * decimal(0.5);
// Update the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@ -137,8 +137,10 @@ inline Vector3 ConeShape::getLocalExtents(decimal margin) const {
// Return the local inertia tensor of the collision shape
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal rSquare = mRadius * mRadius;
decimal diagXZ = 0.15 * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ);
decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0,
0.0, decimal(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ);
}
}; // End of the ReactPhysics3D namespace

10
src/collision/shapes/CylinderShape.h
View File

@ -116,7 +116,7 @@ inline decimal CylinderShape::getHeight() const {
// Set the height
inline void CylinderShape::setHeight(decimal height) {
this->mHalfHeight = height / 2.0;
mHalfHeight = height * decimal(0.5);
}
// Return the local extents in x,y and z direction
@ -126,9 +126,11 @@ inline Vector3 CylinderShape::getLocalExtents(decimal margin) const {
// Return the local inertia tensor of the cylinder
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal height = 2.0 * mHalfHeight;
decimal diag = (1.0 / 12.0) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * mRadius * mRadius, 0.0, 0.0, 0.0, diag);
decimal height = decimal(2.0) * mHalfHeight;
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0,
decimal(0.5) * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag);
}
}; // End of the ReactPhysics3D namespace

6
src/collision/shapes/SphereShape.h
View File

@ -121,8 +121,10 @@ inline Vector3 SphereShape::getLocalExtents(decimal margin) const {
// Return the local inertia tensor of the sphere
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal diag = 0.4 * mass * mRadius * mRadius;
tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
decimal diag = decimal(0.4) * mass * mRadius * mRadius;
tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
}
}; // End of the ReactPhysics3D namespace

42
src/configuration.h
View File

@ -53,52 +53,36 @@ typedef std::pair<bodyindex, bodyindex> bodyindexpair;
// ------------------- Constants ------------------- //
const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
const decimal DECIMAL_SMALLEST = - std::numeric_limits<decimal>::max();
// Maximum decimal value
const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
const decimal DECIMAL_LARGEST = std::numeric_limits<decimal>::max();
// Machine epsilon
const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
// Infinity
const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<decimal>::infinity();
const decimal MACHINE_EPSILON = std::numeric_limits<decimal>::epsilon();
// Pi constant
const reactphysics3d::decimal PI = 3.14159265;
const decimal PI = decimal(3.14159265);
// Default internal constant timestep in seconds
const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0;
const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
// Default friction coefficient for a rigid body
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
// True if the deactivation (sleeping) of inactive bodies is enabled
const bool DEACTIVATION_ENABLED = true;
// // Object margin for collision detection in cm (For GJK-EPA Algorithm)
const reactphysics3d::decimal OBJECT_MARGIN = 0.04;
const decimal OBJECT_MARGIN = decimal(0.04);
// Friction coefficient
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
// Distance threshold for two contact points for a valid persistent contact (in meters)
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
// Distance threshold for two contact points for a valid persistent contact
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;
// Maximum number of bodies
const int NB_MAX_BODIES = 100000;
// Maximum number of constraints
const int NB_MAX_CONSTRAINTS = 100000;
const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
// Number of iterations when solving a LCP problem
const uint DEFAULT_LCP_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;
// Contacts with penetration depth (in meters) larger that this use error correction with projection
const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20;
const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
}

15
src/constraint/Constraint.h
View File

@ -100,21 +100,6 @@ class Constraint {
// Return the type of the constraint
ConstraintType getType() const;
// Compute the jacobian matrix for all mathematical constraints
virtual void computeJacobian(int noConstraint,
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0;
// Compute the lowerbounds values for all the mathematical constraints
virtual void computeLowerBound(int noConstraint,
decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0;
// Compute the upperbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint,
decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0;
// Compute the error values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0;
// Return the number of mathematical constraints
unsigned int getNbConstraints() const;

176
src/constraint/Contact.cpp
View File

@ -1,176 +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. *
* *
********************************************************************************/
// Libraries
#include "Contact.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) {
assert(mPenetrationDepth > 0.0);
// Compute the auxiliary lower and upper bounds
// TODO : Now mC is only the mass of the first body but it is probably wrong
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
mMu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81;
// Compute the friction vectors that span the tangential friction plane
computeFrictionVectors();
}
// Destructor
Contact::~Contact() {
}
// This method computes the jacobian matrix for all mathematical constraints
// The argument "J_sp" is the jacobian matrix of the constraint solver. This method
// fill in this matrix with all the jacobian matrix of the mathematical constraint
// of the contact. The argument "noConstraint", is the row were the method have
// to start to fill in the J_sp matrix.
void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
assert(mBody1);
assert(mBody2);
Vector3 body1Position = mBody1->getTransform().getPosition();
Vector3 body2Position = mBody2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index
Vector3 r1 = mWorldPointOnBody1 - body1Position;
Vector3 r2 = mWorldPointOnBody2 - body2Position;
Vector3 r1CrossN = r1.cross(mNormal);
Vector3 r2CrossN = r2.cross(mNormal);
// Compute the jacobian matrix for the body 1 for the contact constraint
J_sp[currentIndex][0] = -mNormal.getX();
J_sp[currentIndex][1] = -mNormal.getY();
J_sp[currentIndex][2] = -mNormal.getZ();
J_sp[currentIndex][3] = -r1CrossN.getX();
J_sp[currentIndex][4] = -r1CrossN.getY();
J_sp[currentIndex][5] = -r1CrossN.getZ();
// Compute the jacobian matrix for the body 2 for the contact constraint
J_sp[currentIndex][6] = mNormal.getX();
J_sp[currentIndex][7] = mNormal.getY();
J_sp[currentIndex][8] = mNormal.getZ();
J_sp[currentIndex][9] = r2CrossN.getX();
J_sp[currentIndex][10] = r2CrossN.getY();
J_sp[currentIndex][11] = r2CrossN.getZ();
currentIndex++;
// Compute the jacobian matrix for the body 1 for the first friction constraint
Vector3 r1CrossU1 = r1.cross(mFrictionVectors[0]);
Vector3 r2CrossU1 = r2.cross(mFrictionVectors[0]);
Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]);
Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]);
J_sp[currentIndex][0] = -mFrictionVectors[0].getX();
J_sp[currentIndex][1] = -mFrictionVectors[0].getY();
J_sp[currentIndex][2] = -mFrictionVectors[0].getZ();
J_sp[currentIndex][3] = -r1CrossU1.getX();
J_sp[currentIndex][4] = -r1CrossU1.getY();
J_sp[currentIndex][5] = -r1CrossU1.getZ();
// Compute the jacobian matrix for the body 2 for the first friction constraint
J_sp[currentIndex][6] = mFrictionVectors[0].getX();
J_sp[currentIndex][7] = mFrictionVectors[0].getY();
J_sp[currentIndex][8] = mFrictionVectors[0].getZ();
J_sp[currentIndex][9] = r2CrossU1.getX();
J_sp[currentIndex][10] = r2CrossU1.getY();
J_sp[currentIndex][11] = r2CrossU1.getZ();
currentIndex++;
// Compute the jacobian matrix for the body 1 for the second friction constraint
J_sp[currentIndex][0] = -mFrictionVectors[1].getX();
J_sp[currentIndex][1] = -mFrictionVectors[1].getY();
J_sp[currentIndex][2] = -mFrictionVectors[1].getZ();
J_sp[currentIndex][3] = -r1CrossU2.getX();
J_sp[currentIndex][4] = -r1CrossU2.getY();
J_sp[currentIndex][5] = -r1CrossU2.getZ();
// Compute the jacobian matrix for the body 2 for the second friction constraint
J_sp[currentIndex][6] = mFrictionVectors[1].getX();
J_sp[currentIndex][7] = mFrictionVectors[1].getY();
J_sp[currentIndex][8] = mFrictionVectors[1].getZ();
J_sp[currentIndex][9] = r2CrossU2.getX();
J_sp[currentIndex][10] = r2CrossU2.getY();
J_sp[currentIndex][11] = r2CrossU2.getZ();
}
// Compute the lowerbounds values for all the mathematical constraints. The
// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint
lowerBounds[noConstraint + 1] = -mMu_mc_g; // Lower bound for the first friction constraint
lowerBounds[noConstraint + 2] = -mMu_mc_g; // Lower bound for the second friction constraint
}
// Compute the upperbounds values for all the mathematical constraints. The
// argument "upperBounds" is the upperbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint
upperBounds[noConstraint + 1] = mMu_mc_g; // Upper bound for the first friction constraint
upperBounds[noConstraint + 2] = mMu_mc_g; // Upper bound for the second friction constraint
}
// Compute the error values for all the mathematical constraints. The argument
// "errorValues" is the error values vector of the constraint solver and this
// method has to fill in this vector starting from the row "noConstraint"
void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const {
assert(mBody1);
assert(mBody2);
// TODO : Do we need this casting anymore ?
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(mBody1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(mBody2);
assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS);
// Compute the error value for the contact constraint
Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3 velocity2 = rigidBody2->getLinearVelocity();
decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
decimal errorValue = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2));
// Assign the error value to the vector of error values
errorValues[noConstraint] = errorValue; // Error value for contact constraint
errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint
errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint
}

50
src/constraint/ContactPoint.cpp Normal file
View File

@ -0,0 +1,50 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
// Libraries
#include "ContactPoint.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
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
ContactPoint::~ContactPoint() {
}

124
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,16 +52,13 @@
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Contact :
This class represents a collision contact between two bodies in
the physics engine. The contact class inherits from the
Constraint class. Each Contact represent a contact between two bodies
and contains the two contact points on each body. The contact has 3
mathematical constraints (1 for the contact constraint, and 2
for the friction constraints).
Class ContactPoint :
This class represents a collision contact point between two
bodies in the physics engine. The ContactPoint class inherits from
the Constraint class.
-------------------------------------------------------------------
*/
class Contact : public Constraint {
class ContactPoint : public Constraint {
protected :
@ -85,31 +82,29 @@ class Contact : public Constraint {
// Contact point on body 2 in world space
Vector3 mWorldPointOnBody2;
// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
// Two orthogonal vectors that span the tangential friction plane
std::vector<Vector3> mFrictionVectors;
decimal mMu_mc_g;
// -------------------- Methods -------------------- //
// Private copy-constructor
Contact(const Contact& contact);
ContactPoint(const ContactPoint& contact);
// Private assignment operator
Contact& operator=(const Contact& contact);
// Compute the two friction vectors that span the tangential friction plane
void computeFrictionVectors();
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;
@ -135,20 +130,23 @@ class Contact : public Constraint {
// Set the contact world point on body 2
void setWorldPointOnBody2(const Vector3& worldPoint);
// Compute the jacobian matrix for all mathematical constraints
virtual void computeJacobian(int noConstraint,
decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const;
// Return true if the contact is a resting contact
bool getIsRestingContact() const;
// Compute the lowerbounds values for all the mathematical constraints
virtual void computeLowerBound(int noConstraint,
decimal lowerBounds[NB_MAX_CONSTRAINTS]) const;
// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
// Compute the upperbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint,
decimal upperBounds[NB_MAX_CONSTRAINTS]) const;
// Get the first friction vector
Vector3 getFrictionVector1() const;
// Compute the error values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const;
// Set the first friction vector
void setFrictionVector1(const Vector3& frictionVector1);
// Get the second friction vector
Vector3 getFrictionVector2() const;
// Set the second friction vector
void setFrictionVector2(const Vector3& frictionVector2);
// Return the penetration depth
decimal getPenetrationDepth() const;
@ -159,73 +157,89 @@ class Contact : public Constraint {
#endif
};
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
// The two vectors have to be such that : v1 x v2 = contactNormal
inline void Contact::computeFrictionVectors() {
// Delete the current friction vectors
mFrictionVectors.clear();
// Compute the first orthogonal vector
Vector3 vector1 = mNormal.getOneOrthogonalVector();
mFrictionVectors.push_back(vector1);
// Compute the second orthogonal vector using the cross product
mFrictionVectors.push_back(mNormal.cross(vector1));
}
// 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 ContactPoint::getIsRestingContact() const {
return mIsRestingContact;
}
// Set the mIsRestingContact variable
inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
// Get the first friction vector
inline Vector3 ContactPoint::getFrictionVector1() const {
return mFrictionVectors[0];
}
// Set the first friction vector
inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
mFrictionVectors[0] = frictionVector1;
}
// Get the second friction vector
inline Vector3 ContactPoint::getFrictionVector2() const {
return mFrictionVectors[1];
}
// Set the second friction vector
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);
}
#endif
} // End of the ReactPhysics3D namespace
}
#endif

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

540
src/engine/ConstraintSolver.cpp
View File

@ -1,540 +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. *
* *
********************************************************************************/
// Libraries
#include "ConstraintSolver.h"
#include "DynamicsWorld.h"
#include "../body/RigidBody.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
:world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
isErrorCorrectionActive(false) {
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
}
// Initialize the constraint solver before each solving
void ConstraintSolver::initialize() {
Constraint* constraint;
nbConstraints = 0;
nbConstraintsError = 0;
// For each constraint
vector<Constraint*>::iterator it;
for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
constraint = *it;
// If the constraint is active
if (constraint->isActive()) {
activeConstraints.push_back(constraint);
// Add the two bodies of the constraint in the constraintBodies list
constraintBodies.insert(constraint->getBody1());
constraintBodies.insert(constraint->getBody2());
// Fill in the body number maping
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
// Update the size of the jacobian matrix
nbConstraints += constraint->getNbConstraints();
// Update the size of the jacobian matrix for error correction projection
if (constraint->getType() == CONTACT) {
nbConstraintsError++;
}
}
}
// Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size();
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
}
// Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices(decimal dt) {
Constraint* constraint;
decimal oneOverDt = 1.0 / dt;
// For each active constraint
int noConstraint = 0;
int noConstraintError = 0;
for (int c=0; c<activeConstraints.size(); c++) {
constraint = activeConstraints.at(c);
// Fill in the J_sp matrix
constraint->computeJacobian(noConstraint, J_sp);
// Fill in the body mapping matrix
for(int i=0; i<constraint->getNbConstraints(); i++) {
bodyMapping[noConstraint+i][0] = constraint->getBody1();
bodyMapping[noConstraint+i][1] = constraint->getBody2();
}
// Fill in the limit vectors for the constraint
constraint->computeLowerBound(noConstraint, lowerBounds);
constraint->computeUpperBound(noConstraint, upperBounds);
// Fill in the error vector
constraint->computeErrorValue(noConstraint, errorValues);
// Get the cached lambda values of the constraint
for (int i=0; i<constraint->getNbConstraints(); i++) {
lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
}
// If the constraint is a contact
if (constraint->getType() == CONTACT) {
Contact* contact = dynamic_cast<Contact*>(constraint);
decimal penetrationDepth = contact->getPenetrationDepth();
// If error correction with projection is active
if (isErrorCorrectionActive) {
// Fill in the error correction projection parameters
lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
upperBoundsError[noConstraintError] = upperBounds[noConstraint];
for (int i=0; i<12; i++) {
J_spError[noConstraintError][i] = J_sp[noConstraint][i];
}
bodyMappingError[noConstraintError][0] = constraint->getBody1();
bodyMappingError[noConstraintError][1] = constraint->getBody2();
penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
// If the penetration depth is small
if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
// Use the Baumgarte error correction term for contacts instead of
// first order world projection
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
}
noConstraintError++;
}
else { // If error correction with projection is not active
// Add the Baumgarte error correction term for contacts
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
}
}
noConstraint += constraint->getNbConstraints();
}
// For each current body that is implied in some constraint
RigidBody* rigidBody;
Body* body;
uint b=0;
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) {
body = *it;
uint bodyNumber = bodyNumberMapping[body];
// TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody);
// Compute the vector V1 with initial velocities values
Vector3 linearVelocity = rigidBody->getLinearVelocity();
Vector3 angularVelocity = rigidBody->getAngularVelocity();
int bodyIndexArray = 6 * bodyNumber;
V1[bodyIndexArray] = linearVelocity[0];
V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyIndexArray] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector Vconstraint with final constraint velocities
if (isErrorCorrectionActive) {
VconstraintError[bodyIndexArray] = 0.0;
VconstraintError[bodyIndexArray + 1] = 0.0;
VconstraintError[bodyIndexArray + 2] = 0.0;
VconstraintError[bodyIndexArray + 3] = 0.0;
VconstraintError[bodyIndexArray + 4] = 0.0;
VconstraintError[bodyIndexArray + 5] = 0.0;
}
// Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce();
Vector3 externalTorque = rigidBody->getExternalTorque();
Fext[bodyIndexArray] = externalForce[0];
Fext[bodyIndexArray + 1] = externalForce[1];
Fext[bodyIndexArray + 2] = externalForce[2];
Fext[bodyIndexArray + 3] = externalTorque[0];
Fext[bodyIndexArray + 4] = externalTorque[1];
Fext[bodyIndexArray + 5] = externalTorque[2];
// Initialize the mass and inertia tensor matrices
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
Minv_sp_mass_diag[bodyNumber] = 0.0;
// If the motion of the rigid body is enabled
if (rigidBody->getIsMotionEnabled()) {
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
}
}
}
// Compute the vector b
void ConstraintSolver::computeVectorB(decimal dt) {
uint indexBody1, indexBody2;
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<nbConstraints; c++) {
// b = errorValues * oneOverDT;
b[c] = errorValues[c] * oneOverDT;
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
decimal multiplication = 0.0;
int body1ArrayIndex = 6 * indexBody1;
int body2ArrayIndex = 6 * indexBody2;
for (uint i=0; i<6; i++) {
multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
}
b[c] -= multiplication * oneOverDT ;
// Substract J*M^-1*F_ext to the vector b
decimal value1 = 0.0;
decimal value2 = 0.0;
decimal sum1, sum2;
value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
for (uint i=0; i<3; i++) {
sum1 = 0.0;
sum2 = 0.0;
for (uint j=0; j<3; j++) {
sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
}
value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
}
b[c] -= value1 + value2;
}
}
// Compute the vector b for error correction projection
void ConstraintSolver::computeVectorBError(decimal dt) {
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<nbConstraintsError; c++) {
// b = errorValues * oneOverDT;
if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
bError[c] = penetrationDepths[c] * oneOverDT;
}
else {
bError[c] = 0.0;
}
}
}
// Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() {
uint indexConstraintArray, indexBody1, indexBody2;
// For each constraint
for (uint c = 0; c<nbConstraints; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
for (uint i=0; i<3; i++) {
B_sp[0][indexConstraintArray + 3 + i] = 0.0;
B_sp[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
}
}
}
}
// Same as B_sp matrix but for error correction projection and
// therefore, only for contact constraints
void ConstraintSolver::computeMatrixB_spErrorCorrection() {
uint indexConstraintArray, indexBody1, indexBody2;
// For each contact constraint
for (uint c = 0; c<nbConstraintsError; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
for (uint i=0; i<3; i++) {
B_spError[0][indexConstraintArray + 3 + i] = 0.0;
B_spError[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
}
}
}
}
// Compute the vector V_constraint (which corresponds to the constraint part of
// the final V2 vector) according to the formula
// V_constraint = dt * (M^-1 * J^T * lambda)
// Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B.
// This method is called after that the LCP solver has computed lambda
void ConstraintSolver::computeVectorVconstraint(decimal dt) {
uint indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
indexConstraintArray = 6 * i;
for (j=0; j<6; j++) {
Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt;
Vconstraint[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i] * dt;
}
}
}
// Same as computeVectorVconstraint() but for error correction projection
void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
uint indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute M^-1 * J^T * lambda
for (uint i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
indexConstraintArray = 6 * i;
for (j=0; j<6; j++) {
VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCP() {
for (uint i=0; i<nbConstraints; i++) {
lambda[i] = lambdaInit[i];
}
uint indexBody1Array, indexBody2Array;
decimal deltaLambda;
decimal lambdaTemp;
uint i, iter;
// Compute the vector a
computeVectorA();
// For each constraint
for (i=0; i<nbConstraints; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCP; iter++) {
for (i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = b[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambda[i];
lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
deltaLambda = lambda[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
// Solve a LCP problem for error correction projection
// using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCPErrorCorrection() {
for (uint i=0; i<nbConstraintsError; i++) {
lambdaError[i] = 0;
}
uint indexBody1Array, indexBody2Array;
decimal deltaLambda;
decimal lambdaTemp;
uint i, iter;
// Compute the vector a
computeVectorAError();
// For each constraint
for (i=0; i<nbConstraintsError; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
for (i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = bError[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambdaError[i];
lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
deltaLambda = lambdaError[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
// Compute the vector a used in the solve() method
// Note that a = B * lambda
void ConstraintSolver::computeVectorA() {
uint i;
uint indexBody1Array, indexBody2Array;
// Init the vector a with zero values
for (i=0; i<6*nbBodies; i++) {
a[i] = 0.0;
}
for(i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i];
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i];
}
}
}
// Same as computeVectorA() but for error correction projection
void ConstraintSolver::computeVectorAError() {
uint i;
uint indexBody1Array, indexBody2Array;
// Init the vector a with zero values
for (i=0; i<6*nbBodies; i++) {
aError[i] = 0.0;
}
for(i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Cache the lambda values in order to reuse them in the next step
// to initialize the lambda vector
void ConstraintSolver::cacheLambda() {
// For each active constraint
int noConstraint = 0;
for (int c=0; c<activeConstraints.size(); c++) {
// For each constraint of the contact
for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
// Get the lambda value that have just been computed
activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]);
}
noConstraint += activeConstraints[c]->getNbConstraints();
}
}

237
src/engine/ConstraintSolver.h
View File

@ -1,237 +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 CONSTRAINT_SOLVER_H
#define CONSTRAINT_SOLVER_H
// Libraries
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include <map>
#include <set>
// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class DynamicsWorld;
/* -------------------------------------------------------------------
Class ConstrainSolver :
This class represents the constraint solver. The constraint solver
is based on the theory from the paper "Iterative Dynamics with
Temporal Coherence" from Erin Catto. We keep the same notations as
in the paper. The idea is to construct a LCP problem and then solve
it using a Projected Gauss Seidel (PGS) solver.
The idea is to solve the following system for lambda :
JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0
By default, error correction using first world order projections (described
by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and
Versatile Multibody Dynamics Animation") is used for very large penetration
depths. Error correction with projection requires to solve another LCP problem
that is simpler than the above one and by considering only the contact
constraints. The LCP problem for error correction is the following one :
J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0
where "d" is a vector with penetration depths. If the penetration depth of
a given contact is not very large, we use the Baumgarte error correction (see
the paper from Erin Catto).
-------------------------------------------------------------------
*/
class ConstraintSolver {
private:
DynamicsWorld* world; // Reference to the world
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
bool isErrorCorrectionActive; // True if error correction (with world order) is active
uint nbIterationsLCP; // Number of iterations of the LCP solver
uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
uint nbBodies; // Current number of bodies in the physics world
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
// a 1x6 matrix
decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
// a 6x1 matrix
decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints)
decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints)
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
decimal a[6*NB_MAX_BODIES]; // Vector "a"
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections
decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections
decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
// Each cell contains a 6x1 vector with linear and angular velocities
decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque.
void initialize(); // Initialize the constraint solver before each solving
void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(decimal dt); // Compute the vector b
void computeVectorBError(decimal dt); // Compute the vector b for error correction projection
void computeMatrixB_sp(); // Compute the matrix B_sp
void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection
void computeVectorVconstraint(decimal dt); // Compute the vector V2
void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void computeVectorA(); // Compute the vector a used in the solve() method
void computeVectorAError(); // Same as computeVectorA() but for error correction projection
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection
public:
ConstraintSolver(DynamicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(decimal dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction
Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction
void cleanup(); // Cleanup of the constraint solver
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
};
// Return true if the body is in at least one constraint
inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
if(constraintBodies.find(body) != constraintBodies.end()) {
return true;
}
return false;
}
// Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
}
// Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
}
// Return the constrained linear velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
}
// Return the constrained angular velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
}
// Cleanup of the constraint solver
inline void ConstraintSolver::cleanup() {
bodyNumberMapping.clear();
constraintBodies.clear();
activeConstraints.clear();
}
// Set the number of iterations of the LCP solver
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
nbIterationsLCP = nbIterations;
}
// Set the isErrorCorrectionActive value
inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
this->isErrorCorrectionActive = isErrorCorrectionActive;
}
// Solve the current LCP problem
inline void ConstraintSolver::solve(decimal dt) {
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices(dt);
// Compute the vector b
computeVectorB(dt);
if (isErrorCorrectionActive) {
computeVectorBError(dt);
}
// Compute the matrix B
computeMatrixB_sp();
if (isErrorCorrectionActive) {
computeMatrixB_spErrorCorrection();
}
// Solve the LCP problem (computation of lambda)
solveLCP();
if (isErrorCorrectionActive) {
solveLCPErrorCorrection();
}
// Cache the lambda values in order to use them in the next step
cacheLambda();
// Compute the vector Vconstraint
computeVectorVconstraint(dt);
if (isErrorCorrectionActive) {
computeVectorVconstraintError(dt);
}
}
} // End of ReactPhysics3D namespace
#endif

247
src/engine/ContactManifold.cpp Normal file
View File

@ -0,0 +1,247 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
// Libraries
#include "ContactManifold.h"
using namespace reactphysics3d;
// Constructor
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
ContactManifold::~ContactManifold() {
clear();
}
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
// 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 manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
contact->ContactPoint::~ContactPoint();
mMemoryPoolContacts.freeObject(contact);
//removeContact(i);
return;
//break;
}
}
// If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
// Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
}
// 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
mContactPoints[index]->ContactPoint::~ContactPoint();
mMemoryPoolContacts.freeObject(mContactPoints[index]);
// If we don't remove the last index
if (index < mNbContactPoints - 1) {
mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
}
mNbContactPoints--;
}
// 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 ContactManifold::update(const Transform& transform1, const Transform& transform2) {
if (mNbContactPoints == 0) return;
// 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 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 = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i);
}
else {
// 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 (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
}
}
// 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 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<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed. The index of the contact 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. 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
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
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 - 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 - 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 - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area0 < area3) return 3;
else return 0;
}
}
}
// Clear the 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
mContactPoints[i]->ContactPoint::~ContactPoint();
mMemoryPoolContacts.freeObject(mContactPoints[i]);
}
mNbContactPoints = 0;
}

244
src/engine/ContactManifold.h Normal file
View File

@ -0,0 +1,244 @@
/********************************************************************************
* 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 CONTACT_MANIFOLD_H
#define CONTACT_MANIFOLD_H
// Libraries
#include <vector>
#include "../body/Body.h"
#include "../constraint/ContactPoint.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// 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 -------------------- //
// Pointer to the first body
Body* const mBody1;
// 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 mFrictionVector1;
// Second friction vector of the contact manifold
Vector3 mFrictionVector2;
// First friction constraint accumulated impulse
decimal mFrictionImpulse1;
// Second friction constraint accumulated impulse
decimal mFrictionImpulse2;
// Twist friction constraint accumulated impulse
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(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

915
src/engine/ContactSolver.cpp Normal file
View File

@ -0,0 +1,915 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
// Libraries
#include "ContactSolver.h"
#include "DynamicsWorld.h"
#include "../body/RigidBody.h"
using namespace reactphysics3d;
using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
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,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
// Destructor
ContactSolver::~ContactSolver() {
}
// Initialize the constraint solver
void ContactSolver::initialize() {
// TODO : Use better memory allocation here
mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()];
mNbContactManifolds = 0;
// For each contact manifold of the world
vector<ContactManifold*>::iterator it;
for (it = mWorld.getContactManifoldsBeginIterator();
it != mWorld.getContactManifoldsEndIterator(); ++it) {
ContactManifold* externalManifold = *it;
ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
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);
mConstraintBodies.insert(body2);
// Get the position of the two bodies
Vector3 x1 = body1->getTransform().getPosition();
Vector3 x2 = body2->getTransform().getPosition();
// 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) {
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<externalManifold->getNbContactPoints(); c++) {
ContactPointSolver& contactPoint = internalManifold.contacts[c];
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
contactPoint.externalContact = externalContact;
contactPoint.normal = externalContact->getNormal();
contactPoint.r1 = p1 - x1;
contactPoint.r2 = p2 - x2;
contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
contactPoint.isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
contactPoint.penetrationImpulse = 0.0;
contactPoint.friction1Impulse = 0.0;
contactPoint.friction2Impulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 += p1;
internalManifold.frictionPointBody2 += p2;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
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
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
}
else {
// Initialize the accumulated impulses to zero
internalManifold.friction1Impulse = 0.0;
internalManifold.friction2Impulse = 0.0;
internalManifold.frictionTwistImpulse = 0.0;
}
}
mNbContactManifolds++;
}
// Allocated memory for split impulse velocities
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstraintBodies.size() > 0);
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
// Initialize the split impulse velocities
initializeSplitImpulseVelocities();
}
// Initialize the split impulse velocities
void ContactSolver::initializeSplitImpulseVelocities() {
// For each current body that is implied in some constraint
set<RigidBody*>::iterator it;
for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
RigidBody* rigidBody = *it;
assert(rigidBody);
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);
}
}
// Initialize the contact constraints before solving the system
void ContactSolver::initializeContactConstraints() {
// For each contact constraint
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c];
// Get the inertia tensors of both bodies
Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
manifold.normal = Vector3(0.0, 0.0, 0.0);
}
// Get the velocities of the bodies
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<manifold.nbContacts; i++) {
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);
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 (manifold.isBody1Moving) {
massPenetration += manifold.massInverseBody1 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).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);
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// Compute the friction vectors
computeFrictionVectors(deltaV, contactPoint);
contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
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
decimal friction1Mass = 0.0;
decimal friction2Mass = 0.0;
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 (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);
}
// 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
contactPoint.restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) {
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
}
// 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);
}
// Initialize the split impulses to zero
contactPoint.penetrationSplitImpulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
manifold.normal += contactPoint.normal;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
manifold.normal.normalize();
Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
v1 - w1.cross(manifold.r1Friction);
// Compute the friction vectors
computeFrictionVectors(deltaVFrictionPoint, manifold);
// Compute the inverse mass matrix K for the friction constraints at the center of
// the contact manifold
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 (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 (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);
}
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);
}
}
}
// Warm start the solver
// For each constraint, we apply the previous impulse (from the previous step)
// at the beginning. With this technique, we will converge faster towards
// the solution of the linear system
void ContactSolver::warmStart() {
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
bool atLeastOneRestingContactPoint = false;
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// If it is not a new contact (this contact was already existing at last time step)
if (contactPoint.isRestingContact) {
atLeastOneRestingContactPoint = true;
// --------- Penetration --------- //
// 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);
// 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);
// --------- Friction 1 --------- //
// 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);
// --------- Friction 2 --------- //
// Compute the impulse P=J^T * lambda
const Impulse impulseFriction2 = computeFriction2Impulse(
contactPoint.friction2Impulse, contactPoint);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
}
}
else { // If it is a new contact point
// Initialize the accumulated impulses to zero
contactPoint.penetrationImpulse = 0.0;
contactPoint.friction1Impulse = 0.0;
contactPoint.friction2Impulse = 0.0;
}
}
// If we solve the friction constraints at the center of the contact manifold and there is
// at least one resting contact point in the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
// 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);
// ------ 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;
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 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;
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 manifold ------ //
// 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);
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
else { // If it is a new contact manifold
// Initialize the accumulated impulses to zero
contactManifold.friction1Impulse = 0.0;
contactManifold.friction2Impulse = 0.0;
contactManifold.frictionTwistImpulse = 0.0;
}
}
}
// Solve the contact constraints by applying sequential impulses
void ContactSolver::solveContactConstraints() {
decimal deltaLambda;
decimal lambdaTemp;
uint iter;
// For each iteration of the contact solver
for(iter=0; iter<mNbIterations; iter++) {
// 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];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
// --------- Penetration --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
decimal deltaVDotN = deltaV.dot(contactPoint.normal);
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP));
decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) *
contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
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);
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
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);
decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
contactPoint.inversePenetrationMass;
decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
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);
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
// If we do not solve the friction constraints at the center of the contact manifold
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
// --------- Friction 1 --------- //
// Compute J*v
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;
lambdaTemp = contactPoint.friction1Impulse;
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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// --------- Friction 2 --------- //
// Compute J*v
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;
lambdaTemp = contactPoint.friction2Impulse;
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);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
// ------ 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);
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));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
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 ----- //
// Compute J*v
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));
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
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 ------ //
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.frictionTwistImpulse;
contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
std::min(contactManifold.frictionTwistImpulse
+ deltaLambda, frictionLimit));
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * deltaLambda;
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseTwistFriction, contactManifold);
}
}
}
}
// Solve the constraints
void ContactSolver::solve(decimal timeStep) {
// Set the current time step
mTimeStep = timeStep;
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
// Warm start the solver
if (mIsWarmStartingActive) {
warmStart();
}
// Solve the contact constraints
solveContactConstraints();
// Cache the lambda values in order to use them in the next step
storeImpulses();
}
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
ContactManifoldSolver& manifold = mContactConstraints[c];
for (uint i=0; i<manifold.nbContacts; i++) {
ContactPointSolver& contactPoint = manifold.contacts[i];
contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse);
contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse);
contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse);
contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
contactPoint.externalContact->setFrictionVector2(contactPoint.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& manifold) {
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
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& manifold) {
// Update the velocities of the bodies by applying the impulse P
if (manifold.isBody1Moving) {
mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (manifold.isBody2Moving) {
mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
impulse.linearImpulseBody2;
mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}
// 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 such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactPointSolver& contactPoint) const {
assert(contactPoint.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > 0.0) {
// Compute the first friction vector in the direction of the tangent
// velocity difference
contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
}
else {
// Get any orthogonal vector to the normal as the first friction vector
contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
}
// 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();
}
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contact) const {
assert(contact.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > 0.0) {
// Compute the first friction vector in the direction of the tangent
// velocity difference
contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
}
else {
// Get any orthogonal vector to the normal as the first friction vector
contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
}
// The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
}
// Clean up the constraint solver
void ContactSolver::cleanup() {
mConstraintBodies.clear();
if (mContactConstraints != NULL) {
delete[] mContactConstraints;
mContactConstraints = NULL;
}
if (mSplitLinearVelocities != NULL) {
delete[] mSplitLinearVelocities;
mSplitLinearVelocities = NULL;
}
if (mSplitAngularVelocities != NULL) {
delete[] mSplitAngularVelocities;
mSplitAngularVelocities = NULL;
}
}

455
src/engine/ContactSolver.h Normal file
View File

@ -0,0 +1,455 @@
/********************************************************************************
* 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 CONTACT_SOLVER_H
#define CONTACT_SOLVER_H
// Libraries
#include "../constraint/ContactPoint.h"
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include "ContactManifold.h"
#include <map>
#include <set>
// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class DynamicsWorld;
// Structure Impulse
struct Impulse {
public:
const Vector3 linearImpulseBody1;
const Vector3 linearImpulseBody2;
const Vector3 angularImpulseBody1;
const Vector3 angularImpulseBody2;
// Constructor
Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1,
const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
/* -------------------------------------------------------------------
Class ContactSolver :
This class represents the contact solver that is used is solve rigid bodies contacts.
The constraint solver is based on the "Sequential Impulse" technique described by
Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
A constraint between two bodies is represented by a function C(x) which is equal to zero
when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
bodies and b is the constraint bias. We are looking for a force F_c that will act on the
bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
multiplier lambda.
An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
body to change its velocity. The idea of the Sequential Impulse technique is to apply
impulses to bodies of each constraints in order to keep the constraint satisfied.
--- Step 1 ---
First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
we obtain some new velocities v2' that tends to violate the constraints.
v2' = v1 + dt * M^-1 * F_a
where M is a matrix that contains mass and inertia tensor information.
--- Step 2 ---
During the second step, we iterate over all the constraints for a certain number of
iterations and for each constraint we compute the impulse to apply to the bodies needed
so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
P_c is the constraint impulse to apply to the body. Therefore, we have
v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
the bodies to satisfy the constraint.
--- Step 3 ---
In the third step, we integrate the new position x2 of the bodies using the new velocities
v2 computed in the second step with : x2 = x1 + dt * v2.
Note that in the following code (as it is also explained in the slides from Erin Catto),
the value lambda is not only the lagrange multiplier but is the multiplication of the
Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
lambda, we mean (lambda * dt).
We are using the accumulated impulse technique that is also described in the slides from
Erin Catto.
We are also using warm starting. The idea is to warm start the solver at the beginning of
each step by applying the last impulstes for the constraints that we already existing at the
previous step. This allows the iterative solver to converge faster towards the solution.
For contact constraints, we are also using split impulses so that the position correction
that uses Baumgarte stabilization does not change the momentum of the bodies.
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.
-------------------------------------------------------------------
*/
class ContactSolver {
private:
// Structure ContactPointSolver
// Contact solver internal data structure that to store all the
// information relative to a contact point
struct ContactPointSolver {
decimal penetrationImpulse; // Accumulated normal impulse
decimal friction1Impulse; // Accumulated impulse in the 1st friction direction
decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction
decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction
Vector3 normal; // Normal vector of the contact
Vector3 frictionVector1; // First friction vector in the tangent plane
Vector3 frictionVector2; // Second friction vector in the tangent plane
Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane
Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane
Vector3 r1; // Vector from the body 1 center to the contact point
Vector3 r2; // Vector from the body 2 center to the contact point
Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector
Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector
Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector
Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector
Vector3 r1CrossN; // Cross product of r1 with the contact normal
Vector3 r2CrossN; // Cross product of r2 with the contact normal
decimal penetrationDepth; // Penetration depth
decimal restitutionBias; // Velocity restitution bias
decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration
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
ContactPoint* externalContact; // Pointer to the external contact
};
// Structure ContactManifoldSolver
// Contact solver internal data structure to store all the
// information relative to a contact manifold
struct ContactManifoldSolver {
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
decimal massInverseBody2; // Inverse of the mass of body 2
Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1
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[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* externalContactManifold; // Pointer to the external contact manifold
// Variables used when friction constraints are apply at the center of the manifold //
Vector3 normal; // Average normal vector of the contact manifold
Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints
Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints
Vector3 r1Friction; // R1 vector for the friction constraints
Vector3 r2Friction; // R2 vector for the friction constraints
Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector
Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector
Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector
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
Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center
decimal friction1Impulse; // First friction direction impulse at manifold center
decimal friction2Impulse; // Second friction direction impulse at manifold center
decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center
};
// -------------------- Constants --------------------- //
// Beta value for the penetration depth position correction without split impulses
static const decimal BETA;
// Beta value for the penetration depth position correction with split impulses
static const decimal BETA_SPLIT_IMPULSE;
// Slop distance (allowed penetration distance between bodies)
static const decimal SLOP;
// -------------------- Attributes -------------------- //
// Reference to the world
DynamicsWorld& mWorld;
// Number of iterations of the constraints solver
uint mNbIterations;
// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
// Current time step
decimal mTimeStep;
// Contact constraints
ContactManifoldSolver* mContactConstraints;
// Number of contact constraints
uint mNbContactManifolds;
// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
// Pointer to the array of constrained linear velocities (state of the linear velocities
// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
// Pointer to the array of constrained angular velocities (state of the angular velocities
// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
// True if we solve 3 friction constraints at the contact manifold center only
// instead of 2 friction constraints at each contact point
bool mIsSolveFrictionAtContactManifoldCenterActive;
// -------------------- Methods -------------------- //
// Initialize the constraint solver
void initialize();
// Initialize the split impulse velocities
void initializeSplitImpulseVelocities();
// Initialize the contact constraints before solving the system
void initializeContactConstraints();
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void storeImpulses();
// Warm start the solver
void warmStart();
// Solve the contact constraints by applying sequential impulses
void solveContactConstraints();
// Apply an impulse to the two bodies of a constraint
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& manifold);
// Compute the collision restitution factor from the restitution factor of each body
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;
// 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
// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
ContactPointSolver &contactPoint) const;
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
// plane for a contact constraint. The two vectors have to be
// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
// Compute a penetration constraint impulse
const Impulse computePenetrationImpulse(decimal deltaLambda,
const ContactPointSolver& contactPoint) const;
// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda,
const ContactPointSolver& contactPoint) const;
// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda,
const ContactPointSolver& contactPoint) const;
public:
// -------------------- Methods -------------------- //
// Constructor
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
// Destructor
virtual ~ContactSolver();
// Solve the constraints
void solve(decimal timeStep);
// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
// Return the constrained linear velocity of a body after solving the constraints
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
// Return the split linear velocity
Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
// Return the constrained angular velocity of a body after solving the constraints
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
// Return the split angular velocity
Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
// Clean up the constraint solver
void cleanup();
// Set the number of iterations of the constraint solver
void setNbIterationsSolver(uint nbIterations);
// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
// Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
};
// Return true if the body is in at least one constraint
inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
return mConstraintBodies.count(body) == 1;
}
// Return the split linear velocity
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitLinearVelocities[indexBody];
}
// Return the split angular velocity
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitAngularVelocities[indexBody];
}
// Set the number of iterations of the constraint solver
inline void ContactSolver::setNbIterationsSolver(uint nbIterations) {
mNbIterations = nbIterations;
}
// Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive;
}
// Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point
inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mIsSolveFrictionAtContactManifoldCenterActive = isActive;
}
// Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
const RigidBody* body2) const {
decimal restitution1 = body1->getRestitution();
decimal restitution2 = body2->getRestitution();
// Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2;
}
// Compute the mixed friction coefficient from the friction coefficient of each body
inline decimal ContactSolver::computeMixedFrictionCoefficient(const RigidBody* body1,
const RigidBody* body2) const {
// Use the geometric mean to compute the mixed friction coefficient
return sqrt(body1->getFrictionCoefficient() * body2->getFrictionCoefficient());
}
// Compute a penetration constraint impulse
inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
}
// Compute the first friction constraint impulse
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);
}
// 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);
}
}
#endif

209
src/engine/DynamicsWorld.cpp
View File

@ -26,13 +26,15 @@
// Libraries
#include "DynamicsWorld.h"
// We want to use the ReactPhysics3D namespace
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Constructor
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mConstraintSolver(this),
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -46,11 +48,13 @@ DynamicsWorld::~DynamicsWorld() {
(*it).second->OverlappingPair::~OverlappingPair();
mMemoryPoolOverlappingPairs.freeObject((*it).second);
}
// Free the allocated memory for the constrained velocities
cleanupConstrainedVelocitiesArray();
}
// Update the physics simulation
void DynamicsWorld::update() {
bool existCollision = false;
assert(mTimer.getIsRunning());
@ -63,16 +67,20 @@ void DynamicsWorld::update() {
// While the time accumulator is not empty
while(mTimer.isPossibleToTakeStep()) {
existCollision = false;
removeAllContactConstraints();
// Remove all contact manifolds
mContactManifolds.clear();
// Compute the collision detection
if (mCollisionDetection.computeCollisionDetection()) {
existCollision = true;
mCollisionDetection.computeCollisionDetection();
// Solve constraints
mConstraintSolver.solve(mTimer.getTimeStep());
// Initialize the constrained velocities
initConstrainedVelocitiesArray();
// If there are contacts
if (!mContactManifolds.empty()) {
// Solve the contacts
mContactSolver.solve(mTimer.getTimeStep());
}
// Update the timer
@ -82,69 +90,61 @@ void DynamicsWorld::update() {
resetBodiesMovementVariable();
// Update the position and orientation of each body
updateAllBodiesMotion();
updateRigidBodiesPositionAndOrientation();
// Cleanup of the constraint solver
if (existCollision) {
mConstraintSolver.cleanup();
}
// Cleanup of the contact solver
mContactSolver.cleanup();
// Cleanup the constrained velocities
cleanupConstrainedVelocitiesArray();
}
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
}
// Compute the motion of all bodies and update their positions and orientations
// First this method compute the vector V2 = V_constraint + V_forces + V1 where
// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
// V2 is the final velocity after the timestep and V1 is the velocity before the
// timestep.
// After having computed the velocity V2, this method will update the position
// and orientation of each body.
// This method uses the semi-implicit Euler method to update the position and
// orientation of the body
void DynamicsWorld::updateAllBodiesMotion() {
// Update the position and orientation of the rigid bodies
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
decimal dt = mTimer.getTimeStep();
Vector3 newLinearVelocity;
Vector3 newAngularVelocity;
Vector3 linearVelocityErrorCorrection;
Vector3 angularVelocityErrorCorrection;
// For each body of thephysics world
// For each rigid body of the world
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
RigidBody* rigidBody = *it;
assert(rigidBody);
// If the body is able to move
// If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) {
newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body
if (mConstraintSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
// Update the old Transform of the body
rigidBody->updateOldTransform();
// Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used to update the position)
if (mContactSolver.isConstrainedBody(rigidBody)) {
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
}
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
// external forces and torques.
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Get current position and orientation of the body
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
// Add the velocity V1 to the new velocity
newLinearVelocity += rigidBody->getLinearVelocity();
newAngularVelocity += rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
linearVelocityErrorCorrection, angularVelocityErrorCorrection);
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
// Update the AABB of the rigid body
rigidBody->updateAABB();
@ -152,37 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
}
}
// Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
decimal dt = mTimer.getTimeStep();
assert(rigidBody);
// Update the old position and orientation of the body
rigidBody->updateOldTransform();
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Get current position and orientation of the body
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
// Error correction projection
Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
Vector3 newPosition = correctedPosition + newLinVelocity * dt;
Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
}
// Compute and set the interpolation factor to all bodies
void DynamicsWorld::setInterpolationFactorToAllBodies() {
@ -200,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
}
}
// Initialize the constrained velocities array at each step
void DynamicsWorld::initConstrainedVelocitiesArray() {
// TODO : Use better memory allocation here
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
double dt = mTimer.getTimeStep();
// Fill in the mapping of rigid body to their index in the constrained
// velocities arrays
uint i = 0;
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
RigidBody* rigidBody = *it;
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
// TODO : Move it somewhere else
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
i++;
}
}
// Cleanup the constrained velocities array at each step
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
// Clear the constrained velocites
mConstrainedLinearVelocities.clear();
mConstrainedAngularVelocities.clear();
// Clear the rigid body to velocities array index mapping
mMapBodyToConstrainedVelocityIndex.clear();
}
// Apply the gravity force to all bodies of the physics world
void DynamicsWorld::applyGravity() {
@ -262,26 +266,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mMemoryPoolRigidBodies.freeObject(rigidBody);
}
// Remove all collision contacts constraints
// TODO : This method should be in the collision detection class
void DynamicsWorld::removeAllContactConstraints() {
// For all constraints
for (vector<Constraint*>::iterator it = mConstraints.begin(); it != mConstraints.end(); ) {
// Try a downcasting
Contact* contact = dynamic_cast<Contact*>(*it);
// If the constraint is a contact
if (contact) {
// Remove it from the constraints of the physics world
it = mConstraints.erase(it);
}
else {
++it;
}
}
}
// Remove all constraints in the physics world
void DynamicsWorld::removeAllConstraints() {
mConstraints.clear();
@ -321,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
@ -332,9 +316,20 @@ 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;
// Add all the contacts in the contact cache of the two bodies
// to the set of constraints in the physics world
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
addConstraint(overlappingPair->getContact(i));
contactManifold.contacts[i] = overlappingPair->getContact(i);
contactManifold.nbContacts++;
}
*/
// Add the contact manifold to the world
mContactManifolds.push_back(overlappingPair->getContactManifold());
}

104
src/engine/DynamicsWorld.h
View File

@ -29,7 +29,7 @@
// Libraries
#include "CollisionWorld.h"
#include "../collision/CollisionDetection.h"
#include "ConstraintSolver.h"
#include "ContactSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../configuration.h"
@ -53,8 +53,8 @@ class DynamicsWorld : public CollisionWorld {
// Timer of the physics engine
Timer mTimer;
// Constraint solver
ConstraintSolver mConstraintSolver;
// Contact solver
ContactSolver mContactSolver;
// True if the deactivation (sleeping) of inactive bodies is enabled
bool mIsDeactivationActive;
@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld {
// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies;
// List that contains all the current constraints
// All the contact constraints
std::vector<ContactManifold*> mContactManifolds;
// All the constraints (except contact constraints)
std::vector<Constraint*> mConstraints;
// Gravity vector of the world
@ -78,7 +81,18 @@ 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)
std::vector<Vector3> mConstrainedLinearVelocities;
// Array of constrained angular velocities (state of the angular velocities
// after solving the constraints)
std::vector<Vector3> mConstrainedAngularVelocities;
// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
// -------------------- Methods -------------------- //
@ -89,17 +103,21 @@ class DynamicsWorld : public CollisionWorld {
DynamicsWorld& operator=(const DynamicsWorld& world);
// Compute the motion of all bodies and update their positions and orientations
void updateAllBodiesMotion();
void updateRigidBodiesPositionAndOrientation();
// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection,
const Vector3& angularVelocityErrorCorrection);
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity);
// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
// Initialize the constrained velocities array at each step
void initConstrainedVelocitiesArray();
// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
// Apply the gravity force to all bodies
void applyGravity();
@ -137,8 +155,15 @@ public :
// Update the physics simulation
void update();
// Set the number of iterations of the LCP solver
void setNbLCPIterations(uint nbIterations);
// Set the number of iterations of the constraint solver
void setNbIterationsSolver(uint nbIterations);
// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
// Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
// Set the isErrorCorrectionActive value
void setIsErrorCorrectionActive(bool isErrorCorrectionActive);
@ -160,24 +185,33 @@ public :
// Set the isGravityOn attribute
void setIsGratityOn(bool isGravityOn);
// Return the number of rigid bodies in the world
uint getNbRigidBodies() const;
// Add a constraint
void addConstraint(Constraint* constraint);
// Remove a constraint
void removeConstraint(Constraint* constraint);
// Remove all collision contacts constraints
void removeAllContactConstraints();
// Remove all constraints and delete them (free their memory)
void removeAllConstraints();
// Return the number of contact constraints in the world
uint getNbContactManifolds() const;
// Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsBeginIterator();
// Return a end iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator();
// Return a start iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsBeginIterator();
// Return a end iterator on the contact manifolds list
std::vector<ContactManifold*>::iterator getContactManifoldsEndIterator();
// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -197,14 +231,20 @@ inline void DynamicsWorld::stop() {
mTimer.stop();
}
// Set the number of iterations of the LCP solver
inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) {
mConstraintSolver.setNbLCPIterations(nbIterations);
}
// Set the number of iterations of the constraint solver
inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
mContactSolver.setNbIterationsSolver(nbIterations);
}
// Set the isErrorCorrectionActive value
inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
// Activate or Deactivate the split impulses for contacts
inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
mContactSolver.setIsSplitImpulseActive(isActive);
}
// Activate or deactivate the solving of friction constraints at the center of
// the contact manifold instead of solving them at each contact point
inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
}
// Reset the boolean movement variable of each body
@ -265,6 +305,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
mIsGravityOn = isGravityOn;
}
// Return the number of rigid bodies in the world
inline uint DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size();
}
// Return an iterator to the beginning of the bodies of the physics world
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return mRigidBodies.begin();
@ -275,6 +320,11 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
return mRigidBodies.end();
}
// Return the number of contact manifolds in the world
inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size();
}
// Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsBeginIterator() {
return mConstraints.begin();
@ -285,6 +335,16 @@ inline std::vector<Constraint*>::iterator DynamicsWorld::getConstraintsEndIterat
return mConstraints.end();
}
// Return a start iterator on the contact manifolds list
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() {
return mContactManifolds.end();
}
}
#endif

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

227
src/engine/PersistentContactCache.cpp
View File

@ -1,227 +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. *
* *
********************************************************************************/
// Libraries
#include "PersistentContactCache.h"
using namespace reactphysics3d;
// Constructor
PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts)
: mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) {
}
// Destructor
PersistentContactCache::~PersistentContactCache() {
clear();
}
// Add a contact in the cache
void PersistentContactCache::addContact(Contact* contact) {
int indexNewContact = mNbContacts;
// For contact already in the cache
for (uint i=0; i<mNbContacts; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the cache. If it's the case, we do not add the new contact
if (isApproxEqual(contact->getLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) {
// Delete the new contact
contact->Contact::~Contact();
mMemoryPoolContacts.freeObject(contact);
return;
}
}
// If the contact cache is full
if (mNbContacts == MAX_CONTACTS_IN_CACHE) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContact(indexToRemove);
indexNewContact = indexToRemove;
}
// Add the new contact in the cache
mContacts[indexNewContact] = contact;
mNbContacts++;
}
// Remove a contact from the cache
void PersistentContactCache::removeContact(int index) {
assert(index >= 0 && index < mNbContacts);
assert(mNbContacts > 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]);
// If we don't remove the last index
if (index < mNbContacts - 1) {
mContacts[index] = mContacts[mNbContacts - 1];
}
mNbContacts--;
}
// Update the contact cache
// First the world space coordinates of the current contacts in the cache are recomputed from
// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
// the contacts with a too large distance between the contact points in the plane orthogonal to the
// contact normal
void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) {
if (mNbContacts == 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()));
}
// 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 contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore)
if (mContacts[i]->getPenetrationDepth() <= 0.0) {
removeContact(i);
}
else {
// Compute the distance of the two contact points in the place orthogonal to the contact normal
Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth();
Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance threshold, we remove the contact
if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) {
removeContact(i);
}
}
}
}
// Return the index of the contact with the larger penetration depth. This
// corresponding contact will be kept in the cache. The method returns -1 is
// the new contact is the deepest.
int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
int indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<mNbContacts; i++) {
// If the current contact has a larger penetration depth
if (mContacts[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContacts[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed. The index of the contact with the larger penetration
// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
// the different area and we want to keep the contacts with the largest area. The new point is also
// kept.
int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContacts == MAX_CONTACTS_IN_CACHE);
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
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1();
Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[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 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 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 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area0 < area3) return 3;
else return 0;
}
}
}
// Clear the cache
void PersistentContactCache::clear() {
for (uint i=0; i<mNbContacts; 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]);
}
mNbContacts = 0;
}

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 = 0.1;
return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
approxEqual(vector1.getZ(), vector2.getZ(), epsilon));
}
}
#endif

2
src/engine/Timer.h
View File

@ -153,7 +153,7 @@ inline void Timer::start() {
LARGE_INTEGER ticks;
QueryPerformanceFrequency(&ticksPerSecond);
QueryPerformanceCounter(&ticks);
lastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
mLastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart);
#else
// Initialize the lastUpdateTime with the current time in seconds
timeval timeValue;

2
src/mathematics/Transform.h
View File

@ -176,7 +176,7 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform,
const Transform& newTransform,
decimal interpolationFactor) {
Vector3 interPosition = oldTransform.mPosition * (1.0 - interpolationFactor) +
Vector3 interPosition = oldTransform.mPosition * (decimal(1.0) - interpolationFactor) +
newTransform.mPosition * interpolationFactor;
Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation,

25
src/mathematics/Vector3.cpp
View File

@ -26,7 +26,6 @@
// Libraries
#include "Vector3.h"
#include <iostream>
#include <cassert>
#include <vector>
// Namespaces
@ -97,3 +96,27 @@ Vector3 Vector3::getOneOrthogonalVector() const {
//assert(vector1.isUnit());
return vector1;
}
// Return one unit orthogonal vector of the current vector
Vector3 Vector3::getOneUnitOrthogonalVector() const {
assert(!this->isZero());
decimal x = mValues[0];
decimal y = mValues[1];
decimal z = mValues[2];
// Get the minimum element of the vector
Vector3 vectorAbs(fabs(x), fabs(y), fabs(z));
int minElement = vectorAbs.getMinAxis();
if (minElement == 0) {
return Vector3(0.0, -z, y) / sqrt(y*y + z*z);
}
else if (minElement == 1) {
return Vector3(-z, 0.0, x) / sqrt(x*x + z*z);
}
else {
return Vector3(-y, x, 0.0) / sqrt(x*x + y*y);
}
}

33
src/mathematics/Vector3.h
View File

@ -28,6 +28,7 @@
// Libraries
#include <cmath>
#include <cassert>
#include "mathematics_functions.h"
#include "../decimal.h"
@ -95,6 +96,9 @@ class Vector3 {
// Return the corresponding unit vector
Vector3 getUnit() const;
// Return one unit orthogonal vector of the current vector
Vector3 getOneUnitOrthogonalVector() const;
// Return true if the vector is unit and false otherwise
bool isUnit() const;
@ -110,6 +114,9 @@ class Vector3 {
// Cross product of two vectors
Vector3 cross(const Vector3& vector) const;
// Normalize the vector
void normalize();
// Return the corresponding absolute value vector
Vector3 getAbsoluteVector() const;
@ -137,6 +144,9 @@ class Vector3 {
// Overloaded operator for multiplication with a number with assignment
Vector3& operator*=(decimal number);
// Overloaded operator for division by a number with assignment
Vector3& operator/=(decimal number);
// Overloaded operator for value access
decimal& operator[] (int index);
@ -153,6 +163,7 @@ class Vector3 {
friend Vector3 operator-(const Vector3& vector);
friend Vector3 operator*(const Vector3& vector, decimal number);
friend Vector3 operator*(decimal number, const Vector3& vector);
friend Vector3 operator/(const Vector3& vector, decimal number);
};
// Get the x component of the vector
@ -217,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const {
mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]);
}
// Normalize the vector
inline void Vector3::normalize() {
decimal l = length();
assert(l != 0.0);
mValues[0] /= l;
mValues[1] /= l;
mValues[2] /= l;
}
// Return the corresponding absolute value vector
inline Vector3 Vector3::getAbsoluteVector() const {
return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2]));
@ -283,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) {
return *this;
}
// Overloaded operator for division by a number with assignment
inline Vector3& Vector3::operator/=(decimal number) {
mValues[0] /= number;
mValues[1] /= number;
mValues[2] /= number;
return *this;
}
// Overloaded operator for value access
inline decimal& Vector3::operator[] (int index) {
return mValues[index];
@ -313,6 +341,11 @@ inline Vector3 operator*(const Vector3& vector, decimal number) {
return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]);
}
// Overloaded operator for division by a number
inline Vector3 operator/(const Vector3& vector, decimal number) {
return Vector3(vector.mValues[0] / number, vector.mValues[1] / number, vector.mValues[2] / number);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(decimal number, const Vector3& vector) {
return vector * number;