From e84f6468c82061a8b5a6c35c3e28bb9bf11205bd Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 19 Feb 2013 23:16:20 +0100 Subject: [PATCH] Clean the code --- src/body/RigidBody.cpp | 4 +- src/body/RigidBody.h | 18 + src/collision/narrowphase/GJK/GJKAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 2 +- src/collision/shapes/ConeShape.cpp | 2 +- src/collision/shapes/ConeShape.h | 10 +- src/collision/shapes/CylinderShape.h | 10 +- src/collision/shapes/SphereShape.h | 6 +- src/configuration.h | 34 +- src/constraint/Constraint.h | 15 - src/constraint/Contact.cpp | 249 +----- src/constraint/Contact.h | 57 +- src/engine/ContactSolver.cpp | 837 +++++++++++++++++++ src/engine/ContactSolver.h | 459 ++++++++++ src/engine/DynamicsWorld.cpp | 26 +- src/engine/DynamicsWorld.h | 12 +- src/engine/PersistentContactCache.h | 2 +- src/mathematics/Transform.h | 2 +- 18 files changed, 1372 insertions(+), 375 deletions(-) create mode 100644 src/engine/ContactSolver.cpp create mode 100644 src/engine/ContactSolver.h diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 3a084e18..a4500d01 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -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); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 5e6930d6..7c34addd 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -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 diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index b45d5696..b58054d3 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -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; /* ------------------------------------------------------------------- diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 2d39f127..0b0c3708 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -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(); diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index 18436fff..49e622f0 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -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); diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index dfcfc25f..4aaeddb7 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -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 diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 0c20de00..97381e05 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -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 diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index b161d910..afc42bba 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -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 diff --git a/src/configuration.h b/src/configuration.h index fd26fb72..935ef6ff 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -53,43 +53,34 @@ typedef std::pair bodyindexpair; // ------------------- Constants ------------------- // -const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); +const decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); // Maximum decimal value -const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits::max(); +const decimal DECIMAL_LARGEST = std::numeric_limits::max(); // Machine epsilon -const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); - -// Infinity -const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits::infinity(); +const decimal MACHINE_EPSILON = std::numeric_limits::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 diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index ceeafec6..9f5d8508 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -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; diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index e89341b4..61403dfc 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -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(mBody1); - RigidBody* rigidBody2 = dynamic_cast(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(mBody1); - RigidBody* rigidBody2 = dynamic_cast(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 -} diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 05d8f215..d18ef023 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -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 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 diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp new file mode 100644 index 00000000..56329af2 --- /dev/null +++ b/src/engine/ContactSolver.cpp @@ -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::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; cgetWorldPointOnBody1(); + 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(internalContactManifold.nbContacts); + internalContactManifold.frictionPointBody2 /= static_cast(internalContactManifold.nbContacts); + internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1; + internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2; + internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1; + internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2; + + // 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::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 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 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; csetCachedLambda(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; + } +} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h new file mode 100644 index 00000000..6b1d9c61 --- /dev/null +++ b/src/engine/ContactSolver.h @@ -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 +#include + +// 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 mConstraintBodies; + + // Map body to index + std::map 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 diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index f03a48c1..f638bd8d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -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 diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index ae8b685a..51f3b6b1 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -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 diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h index 0419ad71..a1b28a8d 100644 --- a/src/engine/PersistentContactCache.h +++ b/src/engine/PersistentContactCache.h @@ -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)); diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index ecdca9b5..71d82044 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -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,