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

Clean the code

Browse Source
This commit is contained in:
Daniel Chappuis 2013-02-19 23:16:20 +01:00
parent 0695b30704
commit e84f6468c8
18 changed files with 1372 additions and 375 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

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

34
src/configuration.h
View File

@ -53,43 +53,34 @@ 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;
// Friction coefficient
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;
const decimal OBJECT_MARGIN = decimal(0.04);
// Distance threshold for two contact points for a valid persistent contact (in meters)
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03;
// Maximum number of bodies
const int NB_MAX_BODIES = 100000;
// Maximum number of constraints
const int NB_MAX_CONSTRAINTS = 100000;
const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
// Number of iterations when solving a LCP problem
const uint DEFAULT_LCP_ITERATIONS = 15;
const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15;
// Number of iterations when solving a LCP problem for error correction
const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5;
@ -97,9 +88,6 @@ 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;
}
#endif

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;

249
src/constraint/Contact.cpp
View File

@ -37,259 +37,12 @@ Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactIn
mLocalPointOnBody2(contactInfo->localPoint2),
mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
mIsRestingContact(false) {
mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) {
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() {
}
void Contact::computeJacobianPenetration(decimal J_spBody1[6], decimal J_spBody2[6]) {
Vector3 body1Position = mBody1->getTransform().getPosition();
Vector3 body2Position = mBody2->getTransform().getPosition();
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_spBody1[0] = -mNormal.getX();
J_spBody1[1] = -mNormal.getY();
J_spBody1[2] = -mNormal.getZ();
J_spBody1[3] = -r1CrossN.getX();
J_spBody1[4] = -r1CrossN.getY();
J_spBody1[5] = -r1CrossN.getZ();
// Compute the jacobian matrix for the body 2 for the contact constraint
J_spBody2[0] = mNormal.getX();
J_spBody2[1] = mNormal.getY();
J_spBody2[2] = mNormal.getZ();
J_spBody2[3] = r2CrossN.getX();
J_spBody2[4] = r2CrossN.getY();
J_spBody2[5] = r2CrossN.getZ();
}
void Contact::computeJacobianFriction1(decimal J_spBody1[6], decimal J_spBody2[6]) {
Vector3 body1Position = mBody1->getTransform().getPosition();
Vector3 body2Position = mBody2->getTransform().getPosition();
Vector3 r1 = mWorldPointOnBody1 - body1Position;
Vector3 r2 = mWorldPointOnBody2 - body2Position;
// 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]);
J_spBody1[0] = -mFrictionVectors[0].getX();
J_spBody1[1] = -mFrictionVectors[0].getY();
J_spBody1[2] = -mFrictionVectors[0].getZ();
J_spBody1[3] = -r1CrossU1.getX();
J_spBody1[4] = -r1CrossU1.getY();
J_spBody1[5] = -r1CrossU1.getZ();
// Compute the jacobian matrix for the body 2 for the first friction constraint
J_spBody2[0] = mFrictionVectors[0].getX();
J_spBody2[1] = mFrictionVectors[0].getY();
J_spBody2[2] = mFrictionVectors[0].getZ();
J_spBody2[3] = r2CrossU1.getX();
J_spBody2[4] = r2CrossU1.getY();
J_spBody2[5] = r2CrossU1.getZ();
}
void Contact::computeJacobianFriction2(decimal J_spBody1[6], decimal J_spBody2[6]) {
Vector3 body1Position = mBody1->getTransform().getPosition();
Vector3 body2Position = mBody2->getTransform().getPosition();
Vector3 r1 = mWorldPointOnBody1 - body1Position;
Vector3 r2 = mWorldPointOnBody2 - body2Position;
Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]);
Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]);
// Compute the jacobian matrix for the body 1 for the second friction constraint
J_spBody1[0] = -mFrictionVectors[1].getX();
J_spBody1[1] = -mFrictionVectors[1].getY();
J_spBody1[2] = -mFrictionVectors[1].getZ();
J_spBody1[3] = -r1CrossU2.getX();
J_spBody1[4] = -r1CrossU2.getY();
J_spBody1[5] = -r1CrossU2.getZ();
// Compute the jacobian matrix for the body 2 for the second friction constraint
J_spBody2[0] = mFrictionVectors[1].getX();
J_spBody2[1] = mFrictionVectors[1].getY();
J_spBody2[2] = mFrictionVectors[1].getZ();
J_spBody2[3] = r2CrossU2.getX();
J_spBody2[4] = r2CrossU2.getY();
J_spBody2[5] = r2CrossU2.getZ();
}
void Contact::computeLowerBoundPenetration(decimal& lowerBound) {
lowerBound = 0.0;
}
void Contact::computeLowerBoundFriction1(decimal& lowerBound) {
lowerBound = -mMu_mc_g;
}
void Contact::computeLowerBoundFriction2(decimal& lowerBound) {
lowerBound = -mMu_mc_g;
}
void Contact::computeUpperBoundPenetration(decimal& upperBound) {
upperBound = DECIMAL_INFINITY;
}
void Contact::computeUpperBoundFriction1(decimal& upperBound) {
upperBound = mMu_mc_g;
}
void Contact::computeUpperBoundFriction2(decimal& upperBound) {
upperBound = mMu_mc_g;
}
// 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
}
void Contact::computeErrorPenetration(decimal& error) {
// TODO : Do we need this casting anymore ?
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(mBody1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(mBody2);
// Compute the error value for the contact constraint
Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3 velocity2 = rigidBody2->getLinearVelocity();
decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
error = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2));
}
// 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
}

57
src/constraint/Contact.h
View File

@ -53,12 +53,9 @@ 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).
This class represents a collision contact point between two
bodies in the physics engine. The contact class inherits from
the Constraint class.
-------------------------------------------------------------------
*/
class Contact : public Constraint {
@ -90,8 +87,6 @@ class Contact : public Constraint {
// Two orthogonal vectors that span the tangential friction plane
std::vector<Vector3> mFrictionVectors;
decimal mMu_mc_g;
// -------------------- Methods -------------------- //
@ -101,9 +96,6 @@ class Contact : public Constraint {
// Private assignment operator
Contact& operator=(const Contact& contact);
// Compute the two friction vectors that span the tangential friction plane
void computeFrictionVectors();
public :
// -------------------- Methods -------------------- //
@ -156,37 +148,6 @@ class Contact : public Constraint {
// Set the second friction vector
void setFrictionVector2(const Vector3& frictionVector2);
// Compute the jacobian matrix for all mathematical constraints
virtual void computeJacobian(int noConstraint,
decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const;
// Compute the lowerbounds values for all the mathematical constraints
virtual void computeLowerBound(int noConstraint,
decimal lowerBounds[NB_MAX_CONSTRAINTS]) const;
// Compute the upperbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint,
decimal upperBounds[NB_MAX_CONSTRAINTS]) const;
// Compute the error values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const;
void computeErrorPenetration(decimal& error);
void computeJacobianPenetration(decimal J_spBody1[6], decimal J_spBody2[6]);
void computeJacobianFriction1(decimal J_spBody1[6], decimal J_spBody2[6]);
void computeJacobianFriction2(decimal J_spBody1[6], decimal J_spBody2[6]);
void computeLowerBoundPenetration(decimal& lowerBound);
void computeLowerBoundFriction1(decimal& lowerBound);
void computeLowerBoundFriction2(decimal& lowerBound);
void computeUpperBoundPenetration(decimal& upperBound);
void computeUpperBoundFriction1(decimal& upperBound);
void computeUpperBoundFriction2(decimal& upperBound);
// Return the penetration depth
decimal getPenetrationDepth() const;
@ -196,16 +157,6 @@ 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();
mFrictionVectors.push_back(Vector3(0, 0, 0));
mFrictionVectors.push_back(Vector3(0, 0, 0));
}
// Return the normal vector of the contact
inline Vector3 Contact::getNormal() const {
return mNormal;
@ -289,6 +240,6 @@ inline void Contact::draw() const {
}
#endif
} // End of the ReactPhysics3D namespace
}
#endif

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

@ -0,0 +1,837 @@
/********************************************************************************
* 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)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0),
mLinearVelocities(0), mAngularVelocities(0), 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()];
mNbContactConstraints = 0;
// For each contact manifold of the world
vector<ContactManifold>::iterator it;
for (it = mWorld.getContactManifoldsBeginIterator();
it != mWorld.getContactManifoldsEndIterator(); ++it) {
ContactManifold& externalContactManifold = *it;
ContactManifoldSolver& internalContactManifold = mContactConstraints[mNbContactConstraints];
assert(externalContactManifold.nbContacts > 0);
// Get the two bodies of the contact
RigidBody* body1 = externalContactManifold.contacts[0]->getBody1();
RigidBody* body2 = externalContactManifold.contacts[0]->getBody2();
// Fill in the body number maping
mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
// 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();
internalContactManifold.indexBody1 = mMapBodyToIndex[body1];
internalContactManifold.indexBody2 = mMapBodyToIndex[body2];
internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalContactManifold.isBody1Moving = body1->getIsMotionEnabled();
internalContactManifold.isBody2Moving = body2->getIsMotionEnabled();
internalContactManifold.massInverseBody1 = body1->getMassInverse();
internalContactManifold.massInverseBody2 = body2->getMassInverse();
internalContactManifold.nbContacts = externalContactManifold.nbContacts;
internalContactManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
internalContactManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
internalContactManifold.contactManifold = &(*it);
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalContactManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0);
internalContactManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0);
}
// For each contact point of the contact manifold
for (uint c=0; c<externalContactManifold.nbContacts; c++) {
ContactPointSolver& contactPoint = internalContactManifold.contacts[c];
// Get a contact point
Contact* externalContact = externalContactManifold.contacts[c];
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
contactPoint.contact = 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) {
internalContactManifold.frictionPointBody1 += p1;
internalContactManifold.frictionPointBody2 += p2;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
internalContactManifold.frictionPointBody1 /= static_cast<decimal>(internalContactManifold.nbContacts);
internalContactManifold.frictionPointBody2 /= static_cast<decimal>(internalContactManifold.nbContacts);
internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1;
internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2;
internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1;
internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2;
// If warm starting is active
if (mIsWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses
internalContactManifold.friction1Impulse = externalContactManifold.friction1Impulse;
internalContactManifold.friction2Impulse = externalContactManifold.friction2Impulse;
internalContactManifold.frictionTwistImpulse = externalContactManifold.frictionTwistImpulse;
}
else {
// Initialize the accumulated impulses to zero
internalContactManifold.friction1Impulse = 0.0;
internalContactManifold.friction2Impulse = 0.0;
internalContactManifold.frictionTwistImpulse = 0.0;
}
}
mNbContactConstraints++;
}
// Compute the number of bodies that are part of some active constraint
uint nbBodies = mConstraintBodies.size();
// Allocated memory for velocities
// TODO : Use better memory allocation here
mLinearVelocities = new Vector3[nbBodies];
mAngularVelocities = new Vector3[nbBodies];
mSplitLinearVelocities = new Vector3[nbBodies];
mSplitAngularVelocities = new Vector3[nbBodies];
assert(mMapBodyToIndex.size() == nbBodies);
}
// Initialize the constrained bodies
void ContactSolver::initializeBodies() {
// 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 = mMapBodyToIndex[rigidBody];
mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
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<mNbContactConstraints; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
// Get the inertia tensors of both bodies
Matrix3x3& I1 = contactManifold.inverseInertiaTensorBody1;
Matrix3x3& I2 = contactManifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
contactManifold.normal = Vector3(0.0, 0.0, 0.0);
}
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
// For each contact point constraint
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
Contact* externalContact = contactPoint.contact;
// 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);
decimal massPenetration = 0.0;
if (contactManifold.isBody1Moving) {
massPenetration += contactManifold.massInverseBody1 +
((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal);
}
if (contactManifold.isBody2Moving) {
massPenetration += contactManifold.massInverseBody2 +
((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
}
massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0);
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 (contactManifold.isBody1Moving) {
friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1);
friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionVector2);
}
if (contactManifold.isBody2Moving) {
friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1);
friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionVector2);
}
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);
// TODO : Use a constant here
if (deltaVDotN < 1.0f) {
contactPoint.restitutionBias = contactManifold.restitutionFactor * deltaVDotN;
}
// Get the cached lambda values of the constraint
if (mIsWarmStartingActive) {
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) {
contactManifold.normal += contactPoint.normal;
}
}
// If we solve the friction constraints at the center of the contact manifold
if (mIsSolveFrictionAtContactManifoldCenterActive) {
contactManifold.normal.normalize();
Vector3 deltaVFrictionPoint = v2 + w2.cross(contactManifold.r2Friction) -
v1 - w1.cross(contactManifold.r1Friction);
// Compute the friction vectors
computeFrictionVectors(deltaVFrictionPoint, contactManifold);
// Compute the inverse mass matrix K for the friction constraints at the center of
// the contact manifold
contactManifold.r1CrossT1 = contactManifold.r1Friction.cross(contactManifold.frictionVector1);
contactManifold.r1CrossT2 = contactManifold.r1Friction.cross(contactManifold.frictionVector2);
contactManifold.r2CrossT1 = contactManifold.r2Friction.cross(contactManifold.frictionVector1);
contactManifold.r2CrossT2 = contactManifold.r2Friction.cross(contactManifold.frictionVector2);
decimal friction1Mass = 0.0;
decimal friction2Mass = 0.0;
if (contactManifold.isBody1Moving) {
friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT1).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector1);
friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT2).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector2);
}
if (contactManifold.isBody2Moving) {
friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT1).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector1);
friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT2).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector2);
}
friction1Mass > 0.0 ? contactManifold.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0);
friction2Mass > 0.0 ? contactManifold.inverseFriction2Mass = decimal(1.0) / friction2Mass : 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<mNbContactConstraints; 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 manifol ------ //
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse;
const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute the impulse P=J^T * lambda
linearImpulseBody1 = -contactManifold.frictionVector2 * contactManifold.friction2Impulse;
angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse;
angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifol ------ //
// 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
for(iter=0; iter<mNbIterations; iter++) {
// For each constraint
for (uint c=0; c<mNbContactConstraints; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
decimal sumPenetrationImpulse = 0.0;
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[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
if (mIsSplitImpulseActive) {
deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
}
else {
deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
}
lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f);
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, 0.0f);
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);
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);
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);
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);
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 ------ //
// TODO : Put this in the initialization method
decimal K = contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody1 * contactManifold.normal) +
contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody2 * contactManifold.normal);
// Compute J*v
deltaV = w2 - w1;
Jv = deltaV.dot(contactManifold.normal);
// TODO : Compute the inverse mass matrix here for twist friction
deltaLambda = -Jv * (decimal(1.0) / K);
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) {
mTimeStep = timeStep;
// Initialize the solver
initialize();
initializeBodies();
// 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 constraint
for (uint c=0; c<mNbContactConstraints; c++) {
ContactManifoldSolver& contactManifold = mContactConstraints[c];
for (uint i=0; i<contactManifold.nbContacts; i++) {
ContactPointSolver& contactPoint = contactManifold.contacts[i];
contactPoint.contact->setCachedLambda(0, contactPoint.penetrationImpulse);
contactPoint.contact->setCachedLambda(1, contactPoint.friction1Impulse);
contactPoint.contact->setCachedLambda(2, contactPoint.friction2Impulse);
contactPoint.contact->setFrictionVector1(contactPoint.frictionVector1);
contactPoint.contact->setFrictionVector2(contactPoint.frictionVector2);
}
contactManifold.contactManifold->friction1Impulse = contactManifold.friction1Impulse;
contactManifold.contactManifold->friction2Impulse = contactManifold.friction2Impulse;
contactManifold.contactManifold->frictionTwistImpulse = contactManifold.frictionTwistImpulse;
contactManifold.contactManifold->frictionVector1 = contactManifold.frictionVector1;
contactManifold.contactManifold->frictionVector2 = contactManifold.frictionVector2;
}
}
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold) {
// Update the velocities of the bodies by applying the impulse P
if (contactManifold.isBody1Moving) {
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (contactManifold.isBody2Moving) {
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& contactManifold) {
// Update the velocities of the bodies by applying the impulse P
if (contactManifold.isBody1Moving) {
mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
impulse.linearImpulseBody1;
mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (contactManifold.isBody2Moving) {
mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
impulse.linearImpulseBody2;
mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.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 {
// Update the old friction vectors
//contact.oldFrictionVector1 = contact.frictionVector1;
//contact.oldFrictionVector2 = contact.frictionVector2;
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() {
mMapBodyToIndex.clear();
mConstraintBodies.clear();
if (mContactConstraints != 0) {
delete[] mContactConstraints;
mContactConstraints = 0;
}
if (mLinearVelocities != 0) {
delete[] mLinearVelocities;
mLinearVelocities = 0;
}
if (mAngularVelocities != 0) {
delete[] mAngularVelocities;
mAngularVelocities = 0;
}
}

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

@ -0,0 +1,459 @@
/********************************************************************************
* 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 "../constraint/Contact.h"
#include "ContactManifold.h"
#include "../configuration.h"
#include "../constraint/Constraint.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 contacts 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 ContactPoint
// 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
Contact* contact; // Pointer to the external contact
};
// Structure ContactManifold
// Contact solver internal data structure to store all the
// information relative to a contact manifold
struct ContactManifoldSolver {
// TODO : Use a constant for the number of contact points
uint indexBody1; // Index of body 1 in the constraint solver
uint indexBody2; // Index of body 2 in the constraint solver
decimal massInverseBody1; // Inverse of the mass of body 1
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[4]; // Contact point constraints
uint nbContacts; // Number of contact points
decimal restitutionFactor; // Mix of the restitution factor for two bodies
decimal frictionCoefficient; // Mix friction coefficient for the two bodies
ContactManifold* contactManifold; // Pointer to the external contact manifold
// 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
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;
// Array of constrained linear velocities
Vector3* mLinearVelocities;
// Array of constrained angular velocities
Vector3* mAngularVelocities;
// 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 mNbContactConstraints;
// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
// Map body to index
std::map<RigidBody*, uint> mMapBodyToIndex;
// 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 constrained bodies
void initializeBodies();
// 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& contactManifold);
// Apply an impulse to the two bodies of a constraint
void applySplitImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold);
// 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);
// 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 constrained linear velocity of a body after solving the constraints
inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
return mLinearVelocities[indexBody];
}
// Return the split linear velocity
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
return mSplitLinearVelocities[indexBody];
}
// Return the constrained angular velocity of a body after solving the constraints
inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
return mAngularVelocities[indexBody];
}
// Return the split angular velocity
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
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);
}
} // End of ReactPhysics3D namespace
#endif

26
src/engine/DynamicsWorld.cpp
View File

@ -32,7 +32,7 @@ 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),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -68,11 +68,11 @@ void DynamicsWorld::update() {
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
// If there are constraints or contacts
if (!mConstraints.empty() || !mContactManifolds.empty()) {
// If there are contacts
if (!mContactManifolds.empty()) {
// Solve the constraints and contacts
mConstraintSolver.solve(mTimer.getTimeStep());
// Solve the contacts
mContactSolver.solve(mTimer.getTimeStep());
}
// Update the timer
@ -84,8 +84,8 @@ void DynamicsWorld::update() {
// Update the position and orientation of each body
updateAllBodiesMotion();
// Cleanup of the constraint solver
mConstraintSolver.cleanup();
// Cleanup of the contact solver
mContactSolver.cleanup();
}
// Compute and set the interpolation factor to all the bodies
@ -118,10 +118,10 @@ void DynamicsWorld::updateAllBodiesMotion() {
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body
if (mConstraintSolver.isConstrainedBody(*it)) {
if (mContactSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody);
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody);
newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody);
newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody);
}
else {
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
@ -161,9 +161,9 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
rigidBody->setAngularVelocity(newAngVelocity);
// Split velocity (only used to update the position)
if (mConstraintSolver.isConstrainedBody(rigidBody)) {
newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody);
if (mContactSolver.isConstrainedBody(rigidBody)) {
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
}
// Get current position and orientation of the body

12
src/engine/DynamicsWorld.h
View File

@ -30,7 +30,7 @@
#include "CollisionWorld.h"
#include "ContactManifold.h"
#include "../collision/CollisionDetection.h"
#include "ConstraintSolver.h"
#include "ContactSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../configuration.h"
@ -54,8 +54,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;
@ -214,18 +214,18 @@ inline void DynamicsWorld::stop() {
// Set the number of iterations of the constraint solver
inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) {
mConstraintSolver.setNbIterationsSolver(nbIterations);
mContactSolver.setNbIterationsSolver(nbIterations);
}
// Activate or Deactivate the split impulses for contacts
inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) {
mConstraintSolver.setIsSplitImpulseActive(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) {
mConstraintSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
}
// Reset the boolean movement variable of each body

2
src/engine/PersistentContactCache.h
View File

@ -136,7 +136,7 @@ inline Contact* PersistentContactCache::getContact(uint index) const {
// Return true if two vectors are approximatively equal
inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1,
const Vector3& vector2) const {
const decimal epsilon = 0.1;
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));

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,