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/EPA/EdgeEPA.h b/src/collision/narrowphase/EPA/EdgeEPA.h index cd9f06bf..bc3c64d2 100644 --- a/src/collision/narrowphase/EPA/EdgeEPA.h +++ b/src/collision/narrowphase/EPA/EdgeEPA.h @@ -105,6 +105,7 @@ inline int EdgeEPA::getIndex() const { inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { mOwnerTriangle = edge.mOwnerTriangle; mIndex = edge.mIndex; + return *this; } // Return the index of the next counter-clockwise edge of the ownver triangle diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 0dcb4e48..33779795 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include "GJKAlgorithm.h" #include "Simplex.h" -#include "../../../constraint/Contact.h" +#include "../../../constraint/ContactPoint.h" #include "../../../configuration.h" #include #include diff --git a/src/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 a5be511f..e570f2d5 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -53,52 +53,36 @@ 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; +const decimal OBJECT_MARGIN = decimal(0.04); -// Friction coefficient -const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; +// Distance threshold for two contact points for a valid persistent contact (in meters) +const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); -// Distance threshold for two contact points for a valid persistent contact -const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; - -// Maximum number of bodies -const int NB_MAX_BODIES = 100000; - -// Maximum number of constraints -const int NB_MAX_CONSTRAINTS = 100000; +const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); // Number of iterations when solving a LCP problem -const uint DEFAULT_LCP_ITERATIONS = 15; - -// Number of iterations when solving a LCP problem for error correction -const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; - -// True if the error correction projection (first order world) is active in the constraint solver -const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; - -// Contacts with penetration depth (in meters) larger that this use error correction with projection -const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20; +const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15; } 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 deleted file mode 100644 index d00d674c..00000000 --- a/src/constraint/Contact.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "Contact.h" - -using namespace reactphysics3d; -using namespace std; - -// Constructor -Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo) - : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), - mPenetrationDepth(contactInfo->penetrationDepth), - mLocalPointOnBody1(contactInfo->localPoint1), - mLocalPointOnBody2(contactInfo->localPoint2), - mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) { - assert(mPenetrationDepth > 0.0); - - // Compute the auxiliary lower and upper bounds - // TODO : Now mC is only the mass of the first body but it is probably wrong - // TODO : Now g is 9.81 but we should use the true gravity value of the physics world. - mMu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81; - - // Compute the friction vectors that span the tangential friction plane - computeFrictionVectors(); -} - -// Destructor -Contact::~Contact() { - -} - -// This method computes the jacobian matrix for all mathematical constraints -// The argument "J_sp" is the jacobian matrix of the constraint solver. This method -// fill in this matrix with all the jacobian matrix of the mathematical constraint -// of the contact. The argument "noConstraint", is the row were the method have -// to start to fill in the J_sp matrix. -void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const { - assert(mBody1); - assert(mBody2); - - Vector3 body1Position = mBody1->getTransform().getPosition(); - Vector3 body2Position = mBody2->getTransform().getPosition(); - int currentIndex = noConstraint; // Current constraint index - - Vector3 r1 = mWorldPointOnBody1 - body1Position; - Vector3 r2 = mWorldPointOnBody2 - body2Position; - Vector3 r1CrossN = r1.cross(mNormal); - Vector3 r2CrossN = r2.cross(mNormal); - - // Compute the jacobian matrix for the body 1 for the contact constraint - J_sp[currentIndex][0] = -mNormal.getX(); - J_sp[currentIndex][1] = -mNormal.getY(); - J_sp[currentIndex][2] = -mNormal.getZ(); - J_sp[currentIndex][3] = -r1CrossN.getX(); - J_sp[currentIndex][4] = -r1CrossN.getY(); - J_sp[currentIndex][5] = -r1CrossN.getZ(); - - // Compute the jacobian matrix for the body 2 for the contact constraint - J_sp[currentIndex][6] = mNormal.getX(); - J_sp[currentIndex][7] = mNormal.getY(); - J_sp[currentIndex][8] = mNormal.getZ(); - J_sp[currentIndex][9] = r2CrossN.getX(); - J_sp[currentIndex][10] = r2CrossN.getY(); - J_sp[currentIndex][11] = r2CrossN.getZ(); - - currentIndex++; - - // Compute the jacobian matrix for the body 1 for the first friction constraint - Vector3 r1CrossU1 = r1.cross(mFrictionVectors[0]); - Vector3 r2CrossU1 = r2.cross(mFrictionVectors[0]); - Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]); - Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]); - J_sp[currentIndex][0] = -mFrictionVectors[0].getX(); - J_sp[currentIndex][1] = -mFrictionVectors[0].getY(); - J_sp[currentIndex][2] = -mFrictionVectors[0].getZ(); - J_sp[currentIndex][3] = -r1CrossU1.getX(); - J_sp[currentIndex][4] = -r1CrossU1.getY(); - J_sp[currentIndex][5] = -r1CrossU1.getZ(); - - // Compute the jacobian matrix for the body 2 for the first friction constraint - J_sp[currentIndex][6] = mFrictionVectors[0].getX(); - J_sp[currentIndex][7] = mFrictionVectors[0].getY(); - J_sp[currentIndex][8] = mFrictionVectors[0].getZ(); - J_sp[currentIndex][9] = r2CrossU1.getX(); - J_sp[currentIndex][10] = r2CrossU1.getY(); - J_sp[currentIndex][11] = r2CrossU1.getZ(); - - currentIndex++; - - // Compute the jacobian matrix for the body 1 for the second friction constraint - J_sp[currentIndex][0] = -mFrictionVectors[1].getX(); - J_sp[currentIndex][1] = -mFrictionVectors[1].getY(); - J_sp[currentIndex][2] = -mFrictionVectors[1].getZ(); - J_sp[currentIndex][3] = -r1CrossU2.getX(); - J_sp[currentIndex][4] = -r1CrossU2.getY(); - J_sp[currentIndex][5] = -r1CrossU2.getZ(); - - // Compute the jacobian matrix for the body 2 for the second friction constraint - J_sp[currentIndex][6] = mFrictionVectors[1].getX(); - J_sp[currentIndex][7] = mFrictionVectors[1].getY(); - J_sp[currentIndex][8] = mFrictionVectors[1].getZ(); - J_sp[currentIndex][9] = r2CrossU2.getX(); - J_sp[currentIndex][10] = r2CrossU2.getY(); - J_sp[currentIndex][11] = r2CrossU2.getZ(); -} - -// Compute the lowerbounds values for all the mathematical constraints. The -// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and -// this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const { - assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS); - - lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint - lowerBounds[noConstraint + 1] = -mMu_mc_g; // Lower bound for the first friction constraint - lowerBounds[noConstraint + 2] = -mMu_mc_g; // Lower bound for the second friction constraint -} - -// Compute the upperbounds values for all the mathematical constraints. The -// argument "upperBounds" is the upperbounds values vector of the constraint solver and -// this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const { - assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS); - - upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint - upperBounds[noConstraint + 1] = mMu_mc_g; // Upper bound for the first friction constraint - upperBounds[noConstraint + 2] = mMu_mc_g; // Upper bound for the second friction constraint -} - -// Compute the error values for all the mathematical constraints. The argument -// "errorValues" is the error values vector of the constraint solver and this -// method has to fill in this vector starting from the row "noConstraint" -void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const { - assert(mBody1); - assert(mBody2); - - // TODO : Do we need this casting anymore ? - RigidBody* rigidBody1 = dynamic_cast(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/ContactPoint.cpp b/src/constraint/ContactPoint.cpp new file mode 100644 index 00000000..b5b87dcf --- /dev/null +++ b/src/constraint/ContactPoint.cpp @@ -0,0 +1,50 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactPoint.h" + +using namespace reactphysics3d; +using namespace std; + +// Constructor +ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2, + const ContactInfo* contactInfo) + : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), + mPenetrationDepth(contactInfo->penetrationDepth), + mLocalPointOnBody1(contactInfo->localPoint1), + mLocalPointOnBody2(contactInfo->localPoint2), + mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), + mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), + mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { + + assert(mPenetrationDepth > 0.0); + +} + +// Destructor +ContactPoint::~ContactPoint() { + +} diff --git a/src/constraint/Contact.h b/src/constraint/ContactPoint.h similarity index 66% rename from src/constraint/Contact.h rename to src/constraint/ContactPoint.h index 421ffe5c..38379d2c 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/ContactPoint.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef CONTACT_H -#define CONTACT_H +#ifndef CONTACT_POINT_H +#define CONTACT_POINT_H // Libraries #include "Constraint.h" @@ -52,16 +52,13 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- - Class Contact : - This class represents a collision contact between two bodies in - the physics engine. The contact class inherits from the - Constraint class. Each Contact represent a contact between two bodies - and contains the two contact points on each body. The contact has 3 - mathematical constraints (1 for the contact constraint, and 2 - for the friction constraints). + Class ContactPoint : + This class represents a collision contact point between two + bodies in the physics engine. The ContactPoint class inherits from + the Constraint class. ------------------------------------------------------------------- */ -class Contact : public Constraint { +class ContactPoint : public Constraint { protected : @@ -85,31 +82,29 @@ class Contact : public Constraint { // Contact point on body 2 in world space Vector3 mWorldPointOnBody2; + // True if the contact is a resting contact (exists for more than one time step) + bool mIsRestingContact; + // Two orthogonal vectors that span the tangential friction plane std::vector mFrictionVectors; - - decimal mMu_mc_g; // -------------------- Methods -------------------- // // Private copy-constructor - Contact(const Contact& contact); + ContactPoint(const ContactPoint& contact); // Private assignment operator - Contact& operator=(const Contact& contact); - - // Compute the two friction vectors that span the tangential friction plane - void computeFrictionVectors(); + ContactPoint& operator=(const ContactPoint& contact); public : // -------------------- Methods -------------------- // // Constructor - Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); + ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); // Destructor - virtual ~Contact(); + virtual ~ContactPoint(); // Return the normal vector of the contact Vector3 getNormal() const; @@ -135,20 +130,23 @@ class Contact : public Constraint { // Set the contact world point on body 2 void setWorldPointOnBody2(const Vector3& worldPoint); - // Compute the jacobian matrix for all mathematical constraints - virtual void computeJacobian(int noConstraint, - decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; + // Return true if the contact is a resting contact + bool getIsRestingContact() const; - // Compute the lowerbounds values for all the mathematical constraints - virtual void computeLowerBound(int noConstraint, - decimal lowerBounds[NB_MAX_CONSTRAINTS]) const; + // Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); - // Compute the upperbounds values for all the mathematical constraints - virtual void computeUpperBound(int noConstraint, - decimal upperBounds[NB_MAX_CONSTRAINTS]) const; + // Get the first friction vector + Vector3 getFrictionVector1() const; - // Compute the error values for all the mathematical constraints - virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const; + // Set the first friction vector + void setFrictionVector1(const Vector3& frictionVector1); + + // Get the second friction vector + Vector3 getFrictionVector2() const; + + // Set the second friction vector + void setFrictionVector2(const Vector3& frictionVector2); // Return the penetration depth decimal getPenetrationDepth() const; @@ -159,73 +157,89 @@ class Contact : public Constraint { #endif }; -// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane -// The two vectors have to be such that : v1 x v2 = contactNormal -inline void Contact::computeFrictionVectors() { - // Delete the current friction vectors - mFrictionVectors.clear(); - - // Compute the first orthogonal vector - Vector3 vector1 = mNormal.getOneOrthogonalVector(); - mFrictionVectors.push_back(vector1); - - // Compute the second orthogonal vector using the cross product - mFrictionVectors.push_back(mNormal.cross(vector1)); -} - // Return the normal vector of the contact -inline Vector3 Contact::getNormal() const { +inline Vector3 ContactPoint::getNormal() const { return mNormal; } // Set the penetration depth of the contact -inline void Contact::setPenetrationDepth(decimal penetrationDepth) { +inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { this->mPenetrationDepth = penetrationDepth; } // Return the contact point on body 1 -inline Vector3 Contact::getLocalPointOnBody1() const { +inline Vector3 ContactPoint::getLocalPointOnBody1() const { return mLocalPointOnBody1; } // Return the contact point on body 2 -inline Vector3 Contact::getLocalPointOnBody2() const { +inline Vector3 ContactPoint::getLocalPointOnBody2() const { return mLocalPointOnBody2; } // Return the contact world point on body 1 -inline Vector3 Contact::getWorldPointOnBody1() const { +inline Vector3 ContactPoint::getWorldPointOnBody1() const { return mWorldPointOnBody1; } // Return the contact world point on body 2 -inline Vector3 Contact::getWorldPointOnBody2() const { +inline Vector3 ContactPoint::getWorldPointOnBody2() const { return mWorldPointOnBody2; } // Set the contact world point on body 1 -inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 -inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { mWorldPointOnBody2 = worldPoint; } +// Return true if the contact is a resting contact +inline bool ContactPoint::getIsRestingContact() const { + return mIsRestingContact; +} + +// Set the mIsRestingContact variable +inline void ContactPoint::setIsRestingContact(bool isRestingContact) { + mIsRestingContact = isRestingContact; +} + +// Get the first friction vector +inline Vector3 ContactPoint::getFrictionVector1() const { + return mFrictionVectors[0]; +} + +// Set the first friction vector +inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { + mFrictionVectors[0] = frictionVector1; +} + +// Get the second friction vector +inline Vector3 ContactPoint::getFrictionVector2() const { + return mFrictionVectors[1]; +} + +// Set the second friction vector +inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { + mFrictionVectors[1] = frictionVector2; +} + // Return the penetration depth of the contact -inline decimal Contact::getPenetrationDepth() const { +inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; } #ifdef VISUAL_DEBUG -inline void Contact::draw() const { +inline void ContactPoint::draw() const { glColor3f(1.0, 0.0, 0.0); glutSolidSphere(0.3, 20, 20); } #endif -} // End of the ReactPhysics3D namespace +} #endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 697103a4..52203b11 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { collisionBody->CollisionBody::~CollisionBody(); // Remove the collision body from the list of bodies - mBodies.erase(collisionBody); // TOOD : Maybe use a set to make this faster + mBodies.erase(collisionBody); // TODO : Maybe use a set to make this faster // Free the object from the memory pool mMemoryPoolCollisionBodies.freeObject(collisionBody); diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 5d9d01d9..8d105882 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -35,7 +35,7 @@ #include "OverlappingPair.h" #include "../collision/CollisionDetection.h" #include "../constraint/Constraint.h" -#include "../constraint/Contact.h" +#include "../constraint/ContactPoint.h" #include "../memory/MemoryPool.h" // Namespace reactphysics3d diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp deleted file mode 100644 index cd98bb64..00000000 --- a/src/engine/ConstraintSolver.cpp +++ /dev/null @@ -1,540 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "ConstraintSolver.h" -#include "DynamicsWorld.h" -#include "../body/RigidBody.h" - -using namespace reactphysics3d; -using namespace std; - - -// Constructor -ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), - nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION), - isErrorCorrectionActive(false) { - -} - -// Destructor -ConstraintSolver::~ConstraintSolver() { - -} - - // Initialize the constraint solver before each solving -void ConstraintSolver::initialize() { - Constraint* constraint; - - nbConstraints = 0; - nbConstraintsError = 0; - - // For each constraint - vector::iterator it; - for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { - constraint = *it; - - // If the constraint is active - if (constraint->isActive()) { - activeConstraints.push_back(constraint); - - // Add the two bodies of the constraint in the constraintBodies list - constraintBodies.insert(constraint->getBody1()); - constraintBodies.insert(constraint->getBody2()); - - // Fill in the body number maping - bodyNumberMapping.insert(pair(constraint->getBody1(), bodyNumberMapping.size())); - bodyNumberMapping.insert(pair(constraint->getBody2(), bodyNumberMapping.size())); - - // Update the size of the jacobian matrix - nbConstraints += constraint->getNbConstraints(); - - // Update the size of the jacobian matrix for error correction projection - if (constraint->getType() == CONTACT) { - nbConstraintsError++; - } - } - } - - // Compute the number of bodies that are part of some active constraint - nbBodies = bodyNumberMapping.size(); - - assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); - assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS); - assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); -} - -// Fill in all the matrices needed to solve the LCP problem -// Notice that all the active constraints should have been evaluated first -void ConstraintSolver::fillInMatrices(decimal dt) { - Constraint* constraint; - decimal oneOverDt = 1.0 / dt; - - // For each active constraint - int noConstraint = 0; - int noConstraintError = 0; - - for (int c=0; ccomputeJacobian(noConstraint, J_sp); - - // Fill in the body mapping matrix - for(int i=0; igetNbConstraints(); i++) { - bodyMapping[noConstraint+i][0] = constraint->getBody1(); - bodyMapping[noConstraint+i][1] = constraint->getBody2(); - } - - // Fill in the limit vectors for the constraint - constraint->computeLowerBound(noConstraint, lowerBounds); - constraint->computeUpperBound(noConstraint, upperBounds); - - // Fill in the error vector - constraint->computeErrorValue(noConstraint, errorValues); - - // Get the cached lambda values of the constraint - for (int i=0; igetNbConstraints(); i++) { - lambdaInit[noConstraint + i] = constraint->getCachedLambda(i); - } - - // If the constraint is a contact - if (constraint->getType() == CONTACT) { - Contact* contact = dynamic_cast(constraint); - decimal penetrationDepth = contact->getPenetrationDepth(); - - // If error correction with projection is active - if (isErrorCorrectionActive) { - - // Fill in the error correction projection parameters - lowerBoundsError[noConstraintError] = lowerBounds[noConstraint]; - upperBoundsError[noConstraintError] = upperBounds[noConstraint]; - for (int i=0; i<12; i++) { - J_spError[noConstraintError][i] = J_sp[noConstraint][i]; - } - bodyMappingError[noConstraintError][0] = constraint->getBody1(); - bodyMappingError[noConstraintError][1] = constraint->getBody2(); - penetrationDepths[noConstraintError] = contact->getPenetrationDepth(); - - // If the penetration depth is small - if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - // Use the Baumgarte error correction term for contacts instead of - // first order world projection - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); - } - - noConstraintError++; - } - else { // If error correction with projection is not active - // Add the Baumgarte error correction term for contacts - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); - } - } - - noConstraint += constraint->getNbConstraints(); - } - - // For each current body that is implied in some constraint - RigidBody* rigidBody; - Body* body; - uint b=0; - for (set::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) { - body = *it; - uint bodyNumber = bodyNumberMapping[body]; - - // TODO : Use polymorphism and remove this downcasting - rigidBody = dynamic_cast(body); - assert(rigidBody); - - // Compute the vector V1 with initial velocities values - Vector3 linearVelocity = rigidBody->getLinearVelocity(); - Vector3 angularVelocity = rigidBody->getAngularVelocity(); - int bodyIndexArray = 6 * bodyNumber; - V1[bodyIndexArray] = linearVelocity[0]; - V1[bodyIndexArray + 1] = linearVelocity[1]; - V1[bodyIndexArray + 2] = linearVelocity[2]; - V1[bodyIndexArray + 3] = angularVelocity[0]; - V1[bodyIndexArray + 4] = angularVelocity[1]; - V1[bodyIndexArray + 5] = angularVelocity[2]; - - // Compute the vector Vconstraint with final constraint velocities - Vconstraint[bodyIndexArray] = 0.0; - Vconstraint[bodyIndexArray + 1] = 0.0; - Vconstraint[bodyIndexArray + 2] = 0.0; - Vconstraint[bodyIndexArray + 3] = 0.0; - Vconstraint[bodyIndexArray + 4] = 0.0; - Vconstraint[bodyIndexArray + 5] = 0.0; - - // Compute the vector Vconstraint with final constraint velocities - if (isErrorCorrectionActive) { - VconstraintError[bodyIndexArray] = 0.0; - VconstraintError[bodyIndexArray + 1] = 0.0; - VconstraintError[bodyIndexArray + 2] = 0.0; - VconstraintError[bodyIndexArray + 3] = 0.0; - VconstraintError[bodyIndexArray + 4] = 0.0; - VconstraintError[bodyIndexArray + 5] = 0.0; - } - - // Compute the vector with forces and torques values - Vector3 externalForce = rigidBody->getExternalForce(); - Vector3 externalTorque = rigidBody->getExternalTorque(); - Fext[bodyIndexArray] = externalForce[0]; - Fext[bodyIndexArray + 1] = externalForce[1]; - Fext[bodyIndexArray + 2] = externalForce[2]; - Fext[bodyIndexArray + 3] = externalTorque[0]; - Fext[bodyIndexArray + 4] = externalTorque[1]; - Fext[bodyIndexArray + 5] = externalTorque[2]; - - // Initialize the mass and inertia tensor matrices - Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - Minv_sp_mass_diag[bodyNumber] = 0.0; - - // If the motion of the rigid body is enabled - if (rigidBody->getIsMotionEnabled()) { - Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld(); - Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse(); - } - } -} - -// Compute the vector b -void ConstraintSolver::computeVectorB(decimal dt) { - uint indexBody1, indexBody2; - decimal oneOverDT = 1.0 / dt; - - for (uint c = 0; c= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - bError[c] = penetrationDepths[c] * oneOverDT; - } - else { - bError[c] = 0.0; - } - } -} - -// Compute the matrix B_sp -void ConstraintSolver::computeMatrixB_sp() { - uint indexConstraintArray, indexBody1, indexBody2; - - // For each constraint - for (uint c = 0; cgetNbConstraints(); i++) { - // Get the lambda value that have just been computed - activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]); - } - - noConstraint += activeConstraints[c]->getNbConstraints(); - } -} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h deleted file mode 100644 index 2f194254..00000000 --- a/src/engine/ConstraintSolver.h +++ /dev/null @@ -1,237 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H - -// Libraries -#include "../configuration.h" -#include "../constraint/Constraint.h" -#include -#include - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Declarations -class DynamicsWorld; - - -/* ------------------------------------------------------------------- - Class ConstrainSolver : - This class represents the constraint solver. The constraint solver - is based on the theory from the paper "Iterative Dynamics with - Temporal Coherence" from Erin Catto. We keep the same notations as - in the paper. The idea is to construct a LCP problem and then solve - it using a Projected Gauss Seidel (PGS) solver. - - The idea is to solve the following system for lambda : - JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0 - - By default, error correction using first world order projections (described - by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and - Versatile Multibody Dynamics Animation") is used for very large penetration - depths. Error correction with projection requires to solve another LCP problem - that is simpler than the above one and by considering only the contact - constraints. The LCP problem for error correction is the following one : - J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0 - - where "d" is a vector with penetration depths. If the penetration depth of - a given contact is not very large, we use the Baumgarte error correction (see - the paper from Erin Catto). - - ------------------------------------------------------------------- -*/ -class ConstraintSolver { - private: - DynamicsWorld* world; // Reference to the world - std::vector activeConstraints; // Current active constraints in the physics world - bool isErrorCorrectionActive; // True if error correction (with world order) is active - uint nbIterationsLCP; // Number of iterations of the LCP solver - uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction - uint nbConstraints; // Total number of constraints (with the auxiliary constraints) - uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) - uint nbBodies; // Current number of bodies in the physics world - std::set constraintBodies; // Bodies that are implied in some constraint - std::map bodyNumberMapping; // Map a body pointer with its index number - Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference - // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains - // the pointer to the body that correspond to the 1x6 J_ij matrix in the - // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector - Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection - decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints - // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint) - // a 1x6 matrix - decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation - // This array contains for each constraint two 6x1 matrices (one for each body of the constraint) - // a 6x1 matrix - decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints) - decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints) - decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem - decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection - decimal d[NB_MAX_CONSTRAINTS]; // Vector "d" - decimal a[6*NB_MAX_BODIES]; // Vector "a" - decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection - decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection - decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem - decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections - decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver - decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints - decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem - decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem - decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections - decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections - Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix) - decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass - // This is an array of size nbBodies that contains in each cell a 6x6 matrix - decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities - // Each cell contains a 6x1 vector with linear and angular velocities - decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities - decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities - decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques - // Each cell contains a 6x1 vector with external force and torque. - void initialize(); // Initialize the constraint solver before each solving - void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem - void computeVectorB(decimal dt); // Compute the vector b - void computeVectorBError(decimal dt); // Compute the vector b for error correction projection - void computeMatrixB_sp(); // Compute the matrix B_sp - void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection - void computeVectorVconstraint(decimal dt); // Compute the vector V2 - void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection - void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector - void computeVectorA(); // Compute the vector a used in the solve() method - void computeVectorAError(); // Same as computeVectorA() but for error correction projection - void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm - void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection - - public: - ConstraintSolver(DynamicsWorld* world); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(decimal dt); // Solve the current LCP problem - bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint - Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem - Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem - Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction - Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction - void cleanup(); // Cleanup of the constraint solver - void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver - void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value -}; - -// Return true if the body is in at least one constraint -inline bool ConstraintSolver::isConstrainedBody(Body* body) const { - if(constraintBodies.find(body) != constraintBodies.end()) { - return true; - } - return false; -} - -// Return the constrained linear velocity of a body after solving the LCP problem -inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { - assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); -} - -// Return the constrained angular velocity of a body after solving the LCP problem -inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { - assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); -} - -// Return the constrained linear velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]); -} - -// Return the constrained angular velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]); -} - -// Cleanup of the constraint solver -inline void ConstraintSolver::cleanup() { - bodyNumberMapping.clear(); - constraintBodies.clear(); - activeConstraints.clear(); -} - -// Set the number of iterations of the LCP solver -inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { - nbIterationsLCP = nbIterations; -} - - -// Set the isErrorCorrectionActive value -inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - this->isErrorCorrectionActive = isErrorCorrectionActive; -} - - -// Solve the current LCP problem -inline void ConstraintSolver::solve(decimal dt) { - - // Initialize the solver - initialize(); - - // Fill-in all the matrices needed to solve the LCP problem - fillInMatrices(dt); - - // Compute the vector b - computeVectorB(dt); - if (isErrorCorrectionActive) { - computeVectorBError(dt); - } - - // Compute the matrix B - computeMatrixB_sp(); - if (isErrorCorrectionActive) { - computeMatrixB_spErrorCorrection(); - } - - // Solve the LCP problem (computation of lambda) - solveLCP(); - if (isErrorCorrectionActive) { - solveLCPErrorCorrection(); - } - - // Cache the lambda values in order to use them in the next step - cacheLambda(); - - // Compute the vector Vconstraint - computeVectorVconstraint(dt); - if (isErrorCorrectionActive) { - computeVectorVconstraintError(dt); - } -} - -} // End of ReactPhysics3D namespace - -#endif diff --git a/src/engine/ContactManifold.cpp b/src/engine/ContactManifold.cpp new file mode 100644 index 00000000..389a59d6 --- /dev/null +++ b/src/engine/ContactManifold.cpp @@ -0,0 +1,247 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactManifold.h" + +using namespace reactphysics3d; + +// Constructor +ContactManifold::ContactManifold(Body* const body1, Body* const body2, + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0), + mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), + mMemoryPoolContacts(memoryPoolContacts) { + +} + +// Destructor +ContactManifold::~ContactManifold() { + clear(); +} + +// Add a contact point in the manifold +void ContactManifold::addContactPoint(ContactPoint* contact) { + + // For contact already in the manifold + for (uint i=0; igetWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); + if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { + + // Delete the new contact + contact->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(contact); + //removeContact(i); + + return; + //break; + } + } + + // If the contact manifold is full + if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { + int indexMaxPenetration = getIndexOfDeepestPenetration(contact); + int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); + removeContactPoint(indexToRemove); + } + + // Add the new contact point in the manifold + mContactPoints[mNbContactPoints] = contact; + mNbContactPoints++; +} + +// Remove a contact point from the manifold +void ContactManifold::removeContactPoint(int index) { + assert(index >= 0 && index < mNbContactPoints); + assert(mNbContactPoints > 0); + + // Call the destructor explicitly and tell the memory pool that + // the corresponding memory block is now free + mContactPoints[index]->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[index]); + + // If we don't remove the last index + if (index < mNbContactPoints - 1) { + mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; + } + + mNbContactPoints--; +} + +// Update the contact manifold +// First the world space coordinates of the current contacts in the manifold are recomputed from +// the corresponding transforms of the bodies because they have moved. Then we remove the contacts +// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also +// the contacts with a too large distance between the contact points in the plane orthogonal to the +// contact normal +void ContactManifold::update(const Transform& transform1, const Transform& transform2) { + if (mNbContactPoints == 0) return; + + // Update the world coordinates and penetration depth of the contact points in the manifold + for (int i=0; isetWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1()); + mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2()); + mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); + } + + const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD; + + // Remove the contact points that don't represent very well the contact manifold + for (int i=mNbContactPoints-1; i>=0; i--) { + assert(i>= 0 && i < mNbContactPoints); + + // Compute the distance between contact points in the normal direction + decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); + + // If the contacts points are too far from each other in the normal direction + if (distanceNormal > squarePersistentContactThreshold) { + removeContactPoint(i); + } + else { + // Compute the distance of the two contact points in the plane + // orthogonal to the contact normal + Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + + mContactPoints[i]->getNormal() * distanceNormal; + Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; + + // If the orthogonal distance is larger than the valid distance + // threshold, we remove the contact + if (projDifference.lengthSquare() > squarePersistentContactThreshold) { + removeContactPoint(i); + } + } + } +} + +// Return the index of the contact point with the larger penetration depth. This +// corresponding contact will be kept in the cache. The method returns -1 is +// the new contact is the deepest. +int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + int indexMaxPenetrationDepth = -1; + decimal maxPenetrationDepth = newContact->getPenetrationDepth(); + + // For each contact in the cache + for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); + indexMaxPenetrationDepth = i; + } + } + + // Return the index of largest penetration depth + return indexMaxPenetrationDepth; +} + +// Return the index that will be removed. The index of the contact point with the larger penetration +// depth is given as a parameter. This contact won't be removed. Given this contact, we compute +// the different area and we want to keep the contacts with the largest area. The new point is also +// kept. In order to compute the area of a quadrilateral, we use the formula : +// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that +// when we compute this area, we do not calculate it exactly but we +// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, +// this is only a guess that is faster to compute. +int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { + + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + + decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint + decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint + decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint + decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint + + if (indexMaxPenetration != 0) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area0 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 1) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area1 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 2) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area2 = crossProduct.lengthSquare(); + } + if (indexMaxPenetration != 3) { + // Compute the area + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 crossProduct = vector1.cross(vector2); + area3 = crossProduct.lengthSquare(); + } + + // Return the index of the contact to remove + return getMaxArea(area0, area1, area2, area3); +} + +// Return the index of maximum area +int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { + if (area0 < area1) { + if (area1 < area2) { + if (area2 < area3) return 3; + else return 2; + } + else { + if (area1 < area3) return 3; + else return 1; + } + } + else { + if (area0 < area2) { + if (area2 < area3) return 3; + else return 2; + } + else { + if (area0 < area3) return 3; + else return 0; + } + } +} + +// Clear the contact manifold +void ContactManifold::clear() { + for (uint i=0; iContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[i]); + } + mNbContactPoints = 0; +} diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h new file mode 100644 index 00000000..33dff5d8 --- /dev/null +++ b/src/engine/ContactManifold.h @@ -0,0 +1,244 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CONTACT_MANIFOLD_H +#define CONTACT_MANIFOLD_H + +// Libraries +#include +#include "../body/Body.h" +#include "../constraint/ContactPoint.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Constants +const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold + +/* ------------------------------------------------------------------- + Class ContactManifold : + This class represents the set of contact points between two bodies. + The contact manifold is implemented in a way to cache the contact + points among the frames for better stability following the + "Contact Generation" presentation of Erwin Coumans at GDC 2010 + conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). + Some code of this class is based on the implementation of the + btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). + The contacts between two bodies are added one after the other in the cache. + When the cache is full, we have to remove one point. The idea is to keep + the point with the deepest penetration depth and also to keep the + points producing the larger area (for a more stable contact manifold). + The new added point is always kept. + ------------------------------------------------------------------- +*/ +class ContactManifold { + + private: + + // -------------------- Attributes -------------------- // + + // Pointer to the first body + Body* const mBody1; + + // Pointer to the second body + Body* const mBody2; + + // Contact points in the manifold + ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + + // Number of contacts in the cache + uint mNbContactPoints; + + // First friction vector of the contact manifold + Vector3 mFrictionVector1; + + // Second friction vector of the contact manifold + Vector3 mFrictionVector2; + + // First friction constraint accumulated impulse + decimal mFrictionImpulse1; + + // Second friction constraint accumulated impulse + decimal mFrictionImpulse2; + + // Twist friction constraint accumulated impulse + decimal mFrictionTwistImpulse; + + // Reference to the memory pool with the contacts + MemoryPool& mMemoryPoolContacts; + + // -------------------- Methods -------------------- // + + // Private copy-constructor + ContactManifold(const ContactManifold& contactManifold); + + // Private assignment operator + ContactManifold& operator=(const ContactManifold& contactManifold); + + // Return the index of maximum area + int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; + + // Return the index of the contact with the larger penetration depth + int getIndexOfDeepestPenetration(ContactPoint* newContact) const; + + // Return the index that will be removed + int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; + + // Remove a contact point from the manifold + void removeContactPoint(int index); + + // Return true if two vectors are approximatively equal + bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; + + public: + + // -------------------- Methods -------------------- // + + // Constructor + ContactManifold(Body* const mBody1, Body* const mBody2, + MemoryPool& mMemoryPoolContacts); + + // Destructor + ~ContactManifold(); + + // Add a contact point to the manifold + void addContactPoint(ContactPoint* contact); + + // Update the contact manifold + void update(const Transform& transform1, const Transform& transform2); + + // Clear the contact manifold + void clear(); + + // Return the number of contact points in the manifold + uint getNbContactPoints() const; + + // Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + // set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + // Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + // set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + // Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + // Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + // Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + // Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + // Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; + + // Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + // Return a contact point of the manifold + ContactPoint* getContactPoint(uint index) const; +}; + +// Return the number of contact points in the manifold +inline uint ContactManifold::getNbContactPoints() const { + return mNbContactPoints; +} + +// Return the first friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector1() const { + return mFrictionVector1; +} + +// set the first friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { + mFrictionVector1 = frictionVector1; +} + +// Return the second friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector2() const { + return mFrictionVector2; +} + +// set the second friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { + mFrictionVector2 = frictionVector2; +} + +// Return the first friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse1() const { + return mFrictionImpulse1; +} + +// Set the first friction accumulated impulse +inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { + mFrictionImpulse1 = frictionImpulse1; +} + +// Return the second friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse2() const { + return mFrictionImpulse2; +} + +// Set the second friction accumulated impulse +inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { + mFrictionImpulse2 = frictionImpulse2; +} + +// Return the friction twist accumulated impulse +inline decimal ContactManifold::getFrictionTwistImpulse() const { + return mFrictionTwistImpulse; +} + +// Set the friction twist accumulated impulse +inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { + mFrictionTwistImpulse = frictionTwistImpulse; +} + +// Return a contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoint(uint index) const { + assert(index >= 0 && index < mNbContactPoints); + return mContactPoints[index]; +} + +// Return true if two vectors are approximatively equal +inline bool ContactManifold::isApproxEqual(const Vector3& vector1, + const Vector3& vector2) const { + const decimal epsilon = decimal(0.1); + return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && + approxEqual(vector1.getY(), vector2.getY(), epsilon) && + approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); +} + +} +#endif + diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp new file mode 100644 index 00000000..b0dfa4ac --- /dev/null +++ b/src/engine/ContactSolver.cpp @@ -0,0 +1,915 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactSolver.h" +#include "DynamicsWorld.h" +#include "../body/RigidBody.h" + +using namespace reactphysics3d; +using namespace std; + +// Constants initialization +const decimal ContactSolver::BETA = decimal(0.2); +const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); +const decimal ContactSolver::SLOP = decimal(0.01); + +// Constructor +ContactSolver::ContactSolver(DynamicsWorld& world,std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex) + :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), + mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), + mContactConstraints(NULL), + mConstrainedLinearVelocities(constrainedLinearVelocities), + mConstrainedAngularVelocities(constrainedAngularVelocities), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), + mIsWarmStartingActive(true), mIsSplitImpulseActive(true), + mIsSolveFrictionAtContactManifoldCenterActive(true) { + +} + +// Destructor +ContactSolver::~ContactSolver() { + +} + +// Initialize the constraint solver +void ContactSolver::initialize() { + + // TODO : Use better memory allocation here + mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()]; + + mNbContactManifolds = 0; + + // For each contact manifold of the world + vector::iterator it; + for (it = mWorld.getContactManifoldsBeginIterator(); + it != mWorld.getContactManifoldsEndIterator(); ++it) { + + ContactManifold* externalManifold = *it; + + ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds]; + + assert(externalManifold->getNbContactPoints() > 0); + + // Get the two bodies of the contact + RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1(); + RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); + + // Add the two bodies of the constraint in the constraintBodies list + mConstraintBodies.insert(body1); + mConstraintBodies.insert(body2); + + // Get the position of the two bodies + Vector3 x1 = body1->getTransform().getPosition(); + Vector3 x2 = body2->getTransform().getPosition(); + + // Initialize the internal contact manifold structure using the external + // contact manifold + internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + internalManifold.isBody1Moving = body1->getIsMotionEnabled(); + internalManifold.isBody2Moving = body2->getIsMotionEnabled(); + internalManifold.massInverseBody1 = body1->getMassInverse(); + internalManifold.massInverseBody2 = body2->getMassInverse(); + internalManifold.nbContacts = externalManifold->getNbContactPoints(); + internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + internalManifold.externalContactManifold = externalManifold; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + } + + // For each contact point of the contact manifold + for (uint c=0; cgetNbContactPoints(); c++) { + + ContactPointSolver& contactPoint = internalManifold.contacts[c]; + + // Get a contact point + ContactPoint* externalContact = externalManifold->getContactPoint(c); + + // Get the contact point on the two bodies + Vector3 p1 = externalContact->getWorldPointOnBody1(); + Vector3 p2 = externalContact->getWorldPointOnBody2(); + + contactPoint.externalContact = externalContact; + contactPoint.normal = externalContact->getNormal(); + contactPoint.r1 = p1 - x1; + contactPoint.r2 = p2 - x2; + contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); + contactPoint.isRestingContact = externalContact->getIsRestingContact(); + externalContact->setIsRestingContact(true); + contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); + contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1 += p1; + internalManifold.frictionPointBody2 += p2; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); + internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); + internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; + internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; + internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); + internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); + + // If warm starting is active + if (mIsWarmStartingActive) { + + // Initialize the accumulated impulses with the previous step accumulated impulses + internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + } + else { + + // Initialize the accumulated impulses to zero + internalManifold.friction1Impulse = 0.0; + internalManifold.friction2Impulse = 0.0; + internalManifold.frictionTwistImpulse = 0.0; + } + } + + mNbContactManifolds++; + } + + // Allocated memory for split impulse velocities + // TODO : Use better memory allocation here + mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()]; + mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()]; + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); + + assert(mConstraintBodies.size() > 0); + assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); + assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size()); + assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size()); + + // Initialize the split impulse velocities + initializeSplitImpulseVelocities(); +} + +// Initialize the split impulse velocities +void ContactSolver::initializeSplitImpulseVelocities() { + + // For each current body that is implied in some constraint + set::iterator it; + for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { + RigidBody* rigidBody = *it; + assert(rigidBody); + + uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; + + // Initialize the split impulse velocities to zero + mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); + mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); + } +} + +// Initialize the contact constraints before solving the system +void ContactSolver::initializeContactConstraints() { + + // For each contact constraint + for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / + massPenetration : + decimal(0.0); + + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // Compute the friction vectors + computeFrictionVectors(deltaV, contactPoint); + + contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); + contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); + contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); + contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); + + // Compute the inverse mass matrix K for the friction + // constraints at each contact point + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( + contactPoint.frictionVector2); + } + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( + contactPoint.frictionVector2); + } + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / + friction1Mass : + decimal(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / + friction2Mass : + decimal(0.0); + } + + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // at the beginning of the contact. Note that if it is a resting contact (normal + // velocity under a given threshold), we don't add a restitution velocity bias + contactPoint.restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(contactPoint.normal); + if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) { + contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; + } + + // If the warm starting of the contact solver is active + if (mIsWarmStartingActive) { + + // Get the cached accumulated impulses from the previous step + contactPoint.penetrationImpulse = externalContact->getCachedLambda(0); + contactPoint.friction1Impulse = externalContact->getCachedLambda(1); + contactPoint.friction2Impulse = externalContact->getCachedLambda(2); + } + + // Initialize the split impulses to zero + contactPoint.penetrationSplitImpulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + manifold.normal += contactPoint.normal; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + manifold.normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); + + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, manifold); + + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( + manifold.frictionVector2); + } + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( + manifold.frictionVector2); + } + decimal frictionTwistMass = manifold.normal.dot( + manifold.inverseInertiaTensorBody1 * + manifold.normal) + + manifold.normal.dot( + manifold.inverseInertiaTensorBody2 * + manifold.normal); + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass + : decimal(0.0); + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass + : decimal(0.0); + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / + frictionTwistMass : + decimal(0.0); + } + } +} + +// Warm start the solver +// For each constraint, we apply the previous impulse (from the previous step) +// at the beginning. With this technique, we will converge faster towards +// the solution of the linear system +void ContactSolver::warmStart() { + + // For each constraint + for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + + // Compute the Lagrange multiplier lambda + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contactPoint.restitutionBias) * + contactPoint.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + } + lambdaTemp = contactPoint.penetrationImpulse; + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + deltaLambda, decimal(0.0)); + deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, + contactPoint); + + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, contactManifold); + + sumPenetrationImpulse += contactPoint.penetrationImpulse; + + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { + + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - + v1Split - w1Split.cross(contactPoint.r1); + decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * + contactPoint.inversePenetrationMass; + decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = std::max( + contactPoint.penetrationSplitImpulse + + deltaLambdaSplit, decimal(0.0)); + deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + + // Compute the impulse P=J^T * lambda + const Impulse splitImpulsePenetration = computePenetrationImpulse( + deltaLambdaSplit, contactPoint); + + applySplitImpulse(splitImpulsePenetration, contactManifold); + } + + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // --------- Friction 1 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector1); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction1Impulse; + contactPoint.friction1Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, + contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // --------- Friction 2 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector2); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction2Impulse; + contactPoint.friction2Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, + contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + // ------ First friction constraint at the center of the contact manifol ------ // + + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + decimal Jv = deltaV.dot(contactManifold.frictionVector1); + + // Compute the Lagrange multiplier lambda + decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction1Impulse; + contactManifold.friction1Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // ------ Second friction constraint at the center of the contact manifol ----- // + + // Compute J*v + deltaV = v2 + w2.cross(contactManifold.r2Friction) + - v1 - w1.cross(contactManifold.r1Friction); + Jv = deltaV.dot(contactManifold.frictionVector2); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv * contactManifold.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction2Impulse; + contactManifold.friction2Impulse = std::max(-frictionLimit, + std::min(contactManifold.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(contactManifold.normal); + + deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.frictionTwistImpulse; + contactManifold.frictionTwistImpulse = std::max(-frictionLimit, + std::min(contactManifold.frictionTwistImpulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = contactManifold.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); + } + } + } +} + +// Solve the constraints +void ContactSolver::solve(decimal timeStep) { + + // Set the current time step + mTimeStep = timeStep; + + // Initialize the solver + initialize(); + + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); + + // Warm start the solver + if (mIsWarmStartingActive) { + warmStart(); + } + + // Solve the contact constraints + solveContactConstraints(); + + // Cache the lambda values in order to use them in the next step + storeImpulses(); +} + +// Store the computed impulses to use them to +// warm start the solver at the next iteration +void ContactSolver::storeImpulses() { + + // For each contact manifold + for (uint c=0; csetCachedLambda(0, contactPoint.penetrationImpulse); + contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse); + contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse); + + contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); + contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + } + + manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); + manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + } +} + +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applyImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { + + // Update the velocities of the bodies by applying the impulse P + if (manifold.isBody1Moving) { + mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (manifold.isBody2Moving) { + mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { + + // Update the velocities of the bodies by applying the impulse P + if (manifold.isBody1Moving) { + mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (manifold.isBody2Moving) { + mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver& contactPoint) const { + + assert(contactPoint.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contact) const { + + assert(contact.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); +} + +// Clean up the constraint solver +void ContactSolver::cleanup() { + + mConstraintBodies.clear(); + + if (mContactConstraints != NULL) { + delete[] mContactConstraints; + mContactConstraints = NULL; + } + if (mSplitLinearVelocities != NULL) { + delete[] mSplitLinearVelocities; + mSplitLinearVelocities = NULL; + } + if (mSplitAngularVelocities != NULL) { + delete[] mSplitAngularVelocities; + mSplitAngularVelocities = NULL; + } +} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h new file mode 100644 index 00000000..1eb7c059 --- /dev/null +++ b/src/engine/ContactSolver.h @@ -0,0 +1,455 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CONTACT_SOLVER_H +#define CONTACT_SOLVER_H + +// Libraries +#include "../constraint/ContactPoint.h" +#include "../configuration.h" +#include "../constraint/Constraint.h" +#include "ContactManifold.h" +#include +#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 contact solver that is used is solve rigid bodies contacts. + The constraint solver is based on the "Sequential Impulse" technique described by + Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + + A constraint between two bodies is represented by a function C(x) which is equal to zero + when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + bodies and b is the constraint bias. We are looking for a force F_c that will act on the + bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + multiplier lambda. + + An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + body to change its velocity. The idea of the Sequential Impulse technique is to apply + impulses to bodies of each constraints in order to keep the constraint satisfied. + + --- Step 1 --- + + First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + we obtain some new velocities v2' that tends to violate the constraints. + + v2' = v1 + dt * M^-1 * F_a + + where M is a matrix that contains mass and inertia tensor information. + + --- Step 2 --- + + During the second step, we iterate over all the constraints for a certain number of + iterations and for each constraint we compute the impulse to apply to the bodies needed + so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + P_c is the constraint impulse to apply to the body. Therefore, we have + v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + the bodies to satisfy the constraint. + + --- Step 3 --- + + In the third step, we integrate the new position x2 of the bodies using the new velocities + v2 computed in the second step with : x2 = x1 + dt * v2. + + Note that in the following code (as it is also explained in the slides from Erin Catto), + the value lambda is not only the lagrange multiplier but is the multiplication of the + Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + lambda, we mean (lambda * dt). + + We are using the accumulated impulse technique that is also described in the slides from + Erin Catto. + + We are also using warm starting. The idea is to warm start the solver at the beginning of + each step by applying the last impulstes for the constraints that we already existing at the + previous step. This allows the iterative solver to converge faster towards the solution. + + For contact constraints, we are also using split impulses so that the position correction + that uses Baumgarte stabilization does not change the momentum of the bodies. + + There are two ways to apply the friction constraints. Either the friction constraints are + applied at each contact point or they are applied only at the center of the contact manifold + between two bodies. If we solve the friction constraints at each contact point, we need + two constraints (two tangential friction directions) and if we solve the friction + constraints at the center of the contact manifold, we need two constraints for tangential + friction but also another twist friction constraint to prevent spin of the body around the + contact manifold center. + + ------------------------------------------------------------------- +*/ +class ContactSolver { + + private: + + // Structure ContactPointSolver + // Contact solver internal data structure that to store all the + // information relative to a contact point + struct ContactPointSolver { + + decimal penetrationImpulse; // Accumulated normal impulse + decimal friction1Impulse; // Accumulated impulse in the 1st friction direction + decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction + decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction + Vector3 normal; // Normal vector of the contact + Vector3 frictionVector1; // First friction vector in the tangent plane + Vector3 frictionVector2; // Second friction vector in the tangent plane + Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane + Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane + Vector3 r1; // Vector from the body 1 center to the contact point + Vector3 r2; // Vector from the body 2 center to the contact point + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + Vector3 r1CrossN; // Cross product of r1 with the contact normal + Vector3 r2CrossN; // Cross product of r2 with the contact normal + decimal penetrationDepth; // Penetration depth + decimal restitutionBias; // Velocity restitution bias + decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration + decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction + decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction + bool isRestingContact; // True if the contact was existing last time step + ContactPoint* externalContact; // Pointer to the external contact + }; + + // Structure ContactManifoldSolver + // Contact solver internal data structure to store all the + // information relative to a contact manifold + struct ContactManifoldSolver { + + uint indexBody1; // Index of body 1 in the constraint solver + uint indexBody2; // Index of body 2 in the constraint solver + decimal massInverseBody1; // Inverse of the mass of body 1 + decimal massInverseBody2; // Inverse of the mass of body 2 + Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 + bool isBody1Moving; // True if the body 1 is allowed to move + bool isBody2Moving; // True if the body 2 is allowed to move + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];// Contact point constraints + uint nbContacts; // Number of contact points + decimal restitutionFactor; // Mix of the restitution factor for two bodies + decimal frictionCoefficient; // Mix friction coefficient for the two bodies + ContactManifold* externalContactManifold; // Pointer to the external contact manifold + + // Variables used when friction constraints are apply at the center of the manifold // + + Vector3 normal; // Average normal vector of the contact manifold + Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints + Vector3 r1Friction; // R1 vector for the friction constraints + Vector3 r2Friction; // R2 vector for the friction constraints + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + decimal inverseFriction1Mass; // Matrix K for the first friction constraint + decimal inverseFriction2Mass; // Matrix K for the second friction constraint + decimal inverseTwistFrictionMass; // Matrix K for the twist friction constraint + Vector3 frictionVector1; // First friction direction at contact manifold center + Vector3 frictionVector2; // Second friction direction at contact manifold center + Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center + decimal friction1Impulse; // First friction direction impulse at manifold center + decimal friction2Impulse; // Second friction direction impulse at manifold center + decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center + }; + + // -------------------- Constants --------------------- // + + // Beta value for the penetration depth position correction without split impulses + static const decimal BETA; + + // Beta value for the penetration depth position correction with split impulses + static const decimal BETA_SPLIT_IMPULSE; + + // Slop distance (allowed penetration distance between bodies) + static const decimal SLOP; + + // -------------------- Attributes -------------------- // + + // Reference to the world + DynamicsWorld& mWorld; + + // Number of iterations of the constraints solver + uint mNbIterations; + + // Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; + + // Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; + + // Current time step + decimal mTimeStep; + + // Contact constraints + ContactManifoldSolver* mContactConstraints; + + // Number of contact constraints + uint mNbContactManifolds; + + // Constrained bodies + std::set mConstraintBodies; + + // Pointer to the array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector& mConstrainedLinearVelocities; + + // Pointer to the array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector& mConstrainedAngularVelocities; + + // Reference to the map of rigid body to their index in the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; + + // True if the warm starting of the solver is active + bool mIsWarmStartingActive; + + // True if the split impulse position correction is active + bool mIsSplitImpulseActive; + + // True if we solve 3 friction constraints at the contact manifold center only + // instead of 2 friction constraints at each contact point + bool mIsSolveFrictionAtContactManifoldCenterActive; + + // -------------------- Methods -------------------- // + + // Initialize the constraint solver + void initialize(); + + // Initialize the split impulse velocities + void initializeSplitImpulseVelocities(); + + // Initialize the contact constraints before solving the system + void initializeContactConstraints(); + + // Store the computed impulses to use them to + // warm start the solver at the next iteration + void storeImpulses(); + + // Warm start the solver + void warmStart(); + + // Solve the contact constraints by applying sequential impulses + void solveContactConstraints(); + + // Apply an impulse to the two bodies of a constraint + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); + + // Apply an impulse to the two bodies of a constraint + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); + + // Compute the collision restitution factor from the restitution factor of each body + decimal computeMixedRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const; + + // Compute the mixed friction coefficient from the friction coefficient of each body + decimal computeMixedFrictionCoefficient(const RigidBody* body1, + const RigidBody* body2)const; + + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact point constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver &contactPoint) const; + + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contactPoint) const; + + // Compute a penetration constraint impulse + const Impulse computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + // Compute the first friction constraint impulse + const Impulse computeFriction1Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + // Compute the second friction constraint impulse + const Impulse computeFriction2Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + public: + + // -------------------- Methods -------------------- // + + // Constructor + ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex); + + // Destructor + virtual ~ContactSolver(); + + // Solve the constraints + void solve(decimal timeStep); + + // Return true if the body is in at least one constraint + bool isConstrainedBody(RigidBody* body) const; + + // Return the constrained linear velocity of a body after solving the constraints + Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); + + // Return the split linear velocity + Vector3 getSplitLinearVelocityOfBody(RigidBody* body); + + // Return the constrained angular velocity of a body after solving the constraints + Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); + + // Return the split angular velocity + Vector3 getSplitAngularVelocityOfBody(RigidBody* body); + + // Clean up the constraint solver + void cleanup(); + + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + + // Activate or deactivate the solving of friction constraints at the center of + // the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); +}; + +// Return true if the body is in at least one constraint +inline bool ContactSolver::isConstrainedBody(RigidBody* body) const { + return mConstraintBodies.count(body) == 1; +} + +// Return the split linear velocity +inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; + return mSplitLinearVelocities[indexBody]; +} + +// Return the split angular velocity +inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; + return mSplitAngularVelocities[indexBody]; +} + +// Set the number of iterations of the constraint solver +inline void ContactSolver::setNbIterationsSolver(uint nbIterations) { + mNbIterations = nbIterations; +} + +// Activate or Deactivate the split impulses for contacts +inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { + mIsSplitImpulseActive = isActive; +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mIsSolveFrictionAtContactManifoldCenterActive = isActive; +} + +// Compute the collision restitution factor from the restitution factor of each body +inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const { + decimal restitution1 = body1->getRestitution(); + decimal restitution2 = body2->getRestitution(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + +// Compute the mixed friction coefficient from the friction coefficient of each body +inline decimal ContactSolver::computeMixedFrictionCoefficient(const RigidBody* body1, + const RigidBody* body2) const { + // Use the geometric mean to compute the mixed friction coefficient + return sqrt(body1->getFrictionCoefficient() * body2->getFrictionCoefficient()); +} + +// Compute a penetration constraint impulse +inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, + contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +} + +// Compute the first friction constraint impulse +inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector1 * deltaLambda, + -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, + contactPoint.r2CrossT1 * deltaLambda); +} + +// Compute the second friction constraint impulse +inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector2 * deltaLambda, + -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionVector2 * deltaLambda, + contactPoint.r2CrossT2 * deltaLambda); +} + +} + +#endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 1c54d39b..a24147f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -26,13 +26,15 @@ // Libraries #include "DynamicsWorld.h" -// We want to use the ReactPhysics3D namespace +// Namespaces using namespace reactphysics3d; using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) - : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mConstraintSolver(this), + : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), + mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities, + mMapBodyToConstrainedVelocityIndex), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -46,11 +48,13 @@ DynamicsWorld::~DynamicsWorld() { (*it).second->OverlappingPair::~OverlappingPair(); mMemoryPoolOverlappingPairs.freeObject((*it).second); } + + // Free the allocated memory for the constrained velocities + cleanupConstrainedVelocitiesArray(); } // Update the physics simulation void DynamicsWorld::update() { - bool existCollision = false; assert(mTimer.getIsRunning()); @@ -63,16 +67,20 @@ void DynamicsWorld::update() { // While the time accumulator is not empty while(mTimer.isPossibleToTakeStep()) { - existCollision = false; - - removeAllContactConstraints(); + // Remove all contact manifolds + mContactManifolds.clear(); // Compute the collision detection - if (mCollisionDetection.computeCollisionDetection()) { - existCollision = true; + mCollisionDetection.computeCollisionDetection(); - // Solve constraints - mConstraintSolver.solve(mTimer.getTimeStep()); + // Initialize the constrained velocities + initConstrainedVelocitiesArray(); + + // If there are contacts + if (!mContactManifolds.empty()) { + + // Solve the contacts + mContactSolver.solve(mTimer.getTimeStep()); } // Update the timer @@ -82,69 +90,61 @@ void DynamicsWorld::update() { resetBodiesMovementVariable(); // Update the position and orientation of each body - updateAllBodiesMotion(); + updateRigidBodiesPositionAndOrientation(); - // Cleanup of the constraint solver - if (existCollision) { - mConstraintSolver.cleanup(); - } + // Cleanup of the contact solver + mContactSolver.cleanup(); + + // Cleanup the constrained velocities + cleanupConstrainedVelocitiesArray(); } // Compute and set the interpolation factor to all the bodies setInterpolationFactorToAllBodies(); } -// Compute the motion of all bodies and update their positions and orientations -// First this method compute the vector V2 = V_constraint + V_forces + V1 where -// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext) -// V2 is the final velocity after the timestep and V1 is the velocity before the -// timestep. -// After having computed the velocity V2, this method will update the position -// and orientation of each body. -// This method uses the semi-implicit Euler method to update the position and -// orientation of the body -void DynamicsWorld::updateAllBodiesMotion() { +// Update the position and orientation of the rigid bodies +void DynamicsWorld::updateRigidBodiesPositionAndOrientation() { decimal dt = mTimer.getTimeStep(); - Vector3 newLinearVelocity; - Vector3 newAngularVelocity; - Vector3 linearVelocityErrorCorrection; - Vector3 angularVelocityErrorCorrection; - // For each body of thephysics world + // For each rigid body of the world for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { RigidBody* rigidBody = *it; assert(rigidBody); - // If the body is able to move + // If the body is allowed to move if (rigidBody->getIsMotionEnabled()) { - newLinearVelocity.setAllValues(0.0, 0.0, 0.0); - newAngularVelocity.setAllValues(0.0, 0.0, 0.0); - linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); - angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); - // If it's a constrained body - if (mConstraintSolver.isConstrainedBody(*it)) { - // Get the constrained linear and angular velocities from the constraint solver - newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); - newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); - - linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody); - angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody); + // Update the old Transform of the body + rigidBody->updateOldTransform(); + + // Get the constrained velocity + uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody]; + Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; + Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; + + // Update the linear and angular velocity of the body + rigidBody->setLinearVelocity(newLinVelocity); + rigidBody->setAngularVelocity(newAngVelocity); + + // Add the split impulse velocity from Contact Solver (only used to update the position) + if (mContactSolver.isConstrainedBody(rigidBody)) { + newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); + newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); } - // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the - // external forces and torques. - newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + // Get current position and orientation of the body + const Vector3& currentPosition = rigidBody->getTransform().getPosition(); + const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - // Add the velocity V1 to the new velocity - newLinearVelocity += rigidBody->getLinearVelocity(); - newAngularVelocity += rigidBody->getAngularVelocity(); - - // Update the position and the orientation of the body according to the new velocity - updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity, - linearVelocityErrorCorrection, angularVelocityErrorCorrection); + // Compute the new position of the body + Vector3 newPosition = currentPosition + newLinVelocity * dt; + Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; + + // Update the Transform of the body + Transform newTransform(newPosition, newOrientation.getUnit()); + rigidBody->setTransform(newTransform); // Update the AABB of the rigid body rigidBody->updateAABB(); @@ -152,37 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() { } } -// Update the position and orientation of a body -// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new -// orientation of the body -void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { - decimal dt = mTimer.getTimeStep(); - - assert(rigidBody); - - // Update the old position and orientation of the body - rigidBody->updateOldTransform(); - - // Update the linear and angular velocity of the body - rigidBody->setLinearVelocity(newLinVelocity); - rigidBody->setAngularVelocity(newAngVelocity); - - // Get current position and orientation of the body - const Vector3& currentPosition = rigidBody->getTransform().getPosition(); - const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - - // Error correction projection - Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection; - Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt; - - Vector3 newPosition = correctedPosition + newLinVelocity * dt; - Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt; - - Transform newTransform(newPosition, newOrientation.getUnit()); - rigidBody->setTransform(newTransform); -} - // Compute and set the interpolation factor to all bodies void DynamicsWorld::setInterpolationFactorToAllBodies() { @@ -200,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { } } +// Initialize the constrained velocities array at each step +void DynamicsWorld::initConstrainedVelocitiesArray() { + + // TODO : Use better memory allocation here + mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + + double dt = mTimer.getTimeStep(); + + // Fill in the mapping of rigid body to their index in the constrained + // velocities arrays + uint i = 0; + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + RigidBody* rigidBody = *it; + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(rigidBody, i)); + + // TODO : Move it somewhere else + mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce(); + mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + + i++; + } +} + +// Cleanup the constrained velocities array at each step +void DynamicsWorld::cleanupConstrainedVelocitiesArray() { + + // Clear the constrained velocites + mConstrainedLinearVelocities.clear(); + mConstrainedAngularVelocities.clear(); + + // Clear the rigid body to velocities array index mapping + mMapBodyToConstrainedVelocityIndex.clear(); +} + // Apply the gravity force to all bodies of the physics world void DynamicsWorld::applyGravity() { @@ -262,26 +266,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mMemoryPoolRigidBodies.freeObject(rigidBody); } -// Remove all collision contacts constraints -// TODO : This method should be in the collision detection class -void DynamicsWorld::removeAllContactConstraints() { - // For all constraints - for (vector::iterator it = mConstraints.begin(); it != mConstraints.end(); ) { - - // Try a downcasting - Contact* contact = dynamic_cast(*it); - - // If the constraint is a contact - if (contact) { - // Remove it from the constraints of the physics world - it = mConstraints.erase(it); - } - else { - ++it; - } - } -} - // Remove all constraints in the physics world void DynamicsWorld::removeAllConstraints() { mConstraints.clear(); @@ -321,7 +305,7 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const assert(rigidBody2); // Create a new contact - Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo); + ContactPoint* contact = new (mMemoryPoolContacts.allocateObject()) ContactPoint(rigidBody1, rigidBody2, contactInfo); assert(contact); // Get the corresponding overlapping pair @@ -332,9 +316,20 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add the contact to the contact cache of the corresponding overlapping pair overlappingPair->addContact(contact); + // TODO : Remove this + /* + // Create a contact manifold with the contact points of the two bodies + ContactManifold contactManifold; + contactManifold.nbContacts = 0; + // Add all the contacts in the contact cache of the two bodies // to the set of constraints in the physics world for (uint i=0; igetNbContacts(); i++) { - addConstraint(overlappingPair->getContact(i)); + contactManifold.contacts[i] = overlappingPair->getContact(i); + contactManifold.nbContacts++; } + */ + + // Add the contact manifold to the world + mContactManifolds.push_back(overlappingPair->getContactManifold()); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index e31d6fef..7bddf900 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -29,7 +29,7 @@ // Libraries #include "CollisionWorld.h" #include "../collision/CollisionDetection.h" -#include "ConstraintSolver.h" +#include "ContactSolver.h" #include "../body/RigidBody.h" #include "Timer.h" #include "../configuration.h" @@ -53,8 +53,8 @@ class DynamicsWorld : public CollisionWorld { // Timer of the physics engine Timer mTimer; - // Constraint solver - ConstraintSolver mConstraintSolver; + // Contact solver + ContactSolver mContactSolver; // True if the deactivation (sleeping) of inactive bodies is enabled bool mIsDeactivationActive; @@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld { // All the rigid bodies of the physics world std::set mRigidBodies; - // List that contains all the current constraints + // All the contact constraints + std::vector mContactManifolds; + + // All the constraints (except contact constraints) std::vector mConstraints; // Gravity vector of the world @@ -78,7 +81,18 @@ class DynamicsWorld : public CollisionWorld { MemoryPool mMemoryPoolRigidBodies; // Memory pool for the contacts - MemoryPool mMemoryPoolContacts; + MemoryPool mMemoryPoolContacts; + + // Array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector mConstrainedLinearVelocities; + + // Array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector mConstrainedAngularVelocities; + + // Map body to their index in the constrained velocities array + std::map mMapBodyToConstrainedVelocityIndex; // -------------------- Methods -------------------- // @@ -89,17 +103,21 @@ class DynamicsWorld : public CollisionWorld { DynamicsWorld& operator=(const DynamicsWorld& world); // Compute the motion of all bodies and update their positions and orientations - void updateAllBodiesMotion(); + void updateRigidBodiesPositionAndOrientation(); // Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, - const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, - const Vector3& angularVelocityErrorCorrection); + void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, + Vector3 newAngVelocity); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); + // Initialize the constrained velocities array at each step + void initConstrainedVelocitiesArray(); + + // Cleanup the constrained velocities array at each step + void cleanupConstrainedVelocitiesArray(); + // Apply the gravity force to all bodies void applyGravity(); @@ -137,8 +155,15 @@ public : // Update the physics simulation void update(); - // Set the number of iterations of the LCP solver - void setNbLCPIterations(uint nbIterations); + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + + // Activate or deactivate the solving of friction constraints at the center of + // the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); // Set the isErrorCorrectionActive value void setIsErrorCorrectionActive(bool isErrorCorrectionActive); @@ -160,24 +185,33 @@ public : // Set the isGravityOn attribute void setIsGratityOn(bool isGravityOn); + // Return the number of rigid bodies in the world + uint getNbRigidBodies() const; + // Add a constraint void addConstraint(Constraint* constraint); // Remove a constraint void removeConstraint(Constraint* constraint); - // Remove all collision contacts constraints - void removeAllContactConstraints(); - // Remove all constraints and delete them (free their memory) void removeAllConstraints(); + // Return the number of contact constraints in the world + uint getNbContactManifolds() const; + // Return a start iterator on the constraint list std::vector::iterator getConstraintsBeginIterator(); // Return a end iterator on the constraint list std::vector::iterator getConstraintsEndIterator(); + // Return a start iterator on the contact manifolds list + std::vector::iterator getContactManifoldsBeginIterator(); + + // Return a end iterator on the contact manifolds list + std::vector::iterator getContactManifoldsEndIterator(); + // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -197,14 +231,20 @@ inline void DynamicsWorld::stop() { mTimer.stop(); } -// Set the number of iterations of the LCP solver -inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { - mConstraintSolver.setNbLCPIterations(nbIterations); -} +// Set the number of iterations of the constraint solver +inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { + mContactSolver.setNbIterationsSolver(nbIterations); +} -// Set the isErrorCorrectionActive value -inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); +// Activate or Deactivate the split impulses for contacts +inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) { + mContactSolver.setIsSplitImpulseActive(isActive); +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } // Reset the boolean movement variable of each body @@ -265,6 +305,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) { mIsGravityOn = isGravityOn; } +// Return the number of rigid bodies in the world +inline uint DynamicsWorld::getNbRigidBodies() const { + return mRigidBodies.size(); +} + // Return an iterator to the beginning of the bodies of the physics world inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { return mRigidBodies.begin(); @@ -275,6 +320,11 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() return mRigidBodies.end(); } +// Return the number of contact manifolds in the world +inline uint DynamicsWorld::getNbContactManifolds() const { + return mContactManifolds.size(); +} + // Return a start iterator on the constraint list inline std::vector::iterator DynamicsWorld::getConstraintsBeginIterator() { return mConstraints.begin(); @@ -285,6 +335,16 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat return mConstraints.end(); } +// Return a start iterator on the contact manifolds list +inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { + return mContactManifolds.begin(); +} + +// Return a end iterator on the contact manifolds list +inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { + return mContactManifolds.end(); +} + } #endif diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 933dac4e..cfd0aef4 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,11 +31,11 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts), + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts), mCachedSeparatingAxis(1.0, 1.0, 1.0) { -} +} // Destructor OverlappingPair::~OverlappingPair() { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index f8d31aff..d8d977eb 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -27,7 +27,7 @@ #define OVERLAPPING_PAIR_H // Libraries -#include "PersistentContactCache.h" +#include "ContactManifold.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -53,8 +53,8 @@ class OverlappingPair { // Pointer to the second body of the contact CollisionBody* const mBody2; - // Persistent contact cache - PersistentContactCache mContactsCache; + // Persistent contact manifold + ContactManifold mContactManifold; // Cached previous separating axis Vector3 mCachedSeparatingAxis; @@ -73,7 +73,7 @@ class OverlappingPair { // Constructor OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts); + MemoryPool& memoryPoolContacts); // Destructor ~OverlappingPair(); @@ -85,7 +85,7 @@ class OverlappingPair { CollisionBody* const getBody2() const; // Add a contact to the contact cache - void addContact(Contact* contact); + void addContact(ContactPoint* contact); // Update the contact cache void update(); @@ -99,8 +99,12 @@ class OverlappingPair { // Return the number of contacts in the cache uint getNbContacts() const; + // Return the contact manifold + ContactManifold* getContactManifold(); + // Return a contact of the cache - Contact* getContact(uint index) const; + // TODO : Maybe remove this method + ContactPoint* getContact(uint index) const; }; // Return the pointer to first body @@ -113,14 +117,14 @@ inline CollisionBody* const OverlappingPair::getBody2() const { return mBody2; } -// Add a contact to the contact cache -inline void OverlappingPair::addContact(Contact* contact) { - mContactsCache.addContact(contact); +// Add a contact to the contact manifold +inline void OverlappingPair::addContact(ContactPoint* contact) { + mContactManifold.addContactPoint(contact); } -// Update the contact cache +// Update the contact manifold inline void OverlappingPair::update() { - mContactsCache.update(mBody1->getTransform(), mBody2->getTransform()); + mContactManifold.update(mBody1->getTransform(), mBody2->getTransform()); } // Return the cached separating axis @@ -136,12 +140,17 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { // Return the number of contacts in the cache inline uint OverlappingPair::getNbContacts() const { - return mContactsCache.getNbContacts(); + return mContactManifold.getNbContactPoints(); +} + +// Return the contact manifold +inline ContactManifold* OverlappingPair::getContactManifold() { + return &mContactManifold; } // Return a contact of the cache -inline Contact* OverlappingPair::getContact(uint index) const { - return mContactsCache.getContact(index); +inline ContactPoint* OverlappingPair::getContact(uint index) const { + return mContactManifold.getContactPoint(index); } } // End of the ReactPhysics3D namespace diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp deleted file mode 100644 index 36bdbc45..00000000 --- a/src/engine/PersistentContactCache.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "PersistentContactCache.h" - -using namespace reactphysics3d; - -// Constructor -PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) { - -} - -// Destructor -PersistentContactCache::~PersistentContactCache() { - clear(); -} - -// Add a contact in the cache -void PersistentContactCache::addContact(Contact* contact) { - - int indexNewContact = mNbContacts; - - // For contact already in the cache - for (uint i=0; igetLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) { - // Delete the new contact - contact->Contact::~Contact(); - mMemoryPoolContacts.freeObject(contact); - - return; - } - } - - // If the contact cache is full - if (mNbContacts == MAX_CONTACTS_IN_CACHE) { - int indexMaxPenetration = getIndexOfDeepestPenetration(contact); - int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContact(indexToRemove); - indexNewContact = indexToRemove; - } - - // Add the new contact in the cache - mContacts[indexNewContact] = contact; - mNbContacts++; -} - -// Remove a contact from the cache -void PersistentContactCache::removeContact(int index) { - assert(index >= 0 && index < mNbContacts); - assert(mNbContacts > 0); - - // Call the destructor explicitly and tell the memory pool that - // the corresponding memory block is now free - mContacts[index]->Contact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[index]); - - // If we don't remove the last index - if (index < mNbContacts - 1) { - mContacts[index] = mContacts[mNbContacts - 1]; - } - - mNbContacts--; -} - -// Update the contact cache -// First the world space coordinates of the current contacts in the cache are recomputed from -// the corresponding transforms of the bodies because they have moved. Then we remove the contacts -// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also -// the contacts with a too large distance between the contact points in the plane orthogonal to the -// contact normal -void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) { - if (mNbContacts == 0) return; - - // Update the world coordinates and penetration depth of the contacts in the cache - for (int i=0; isetWorldPointOnBody1(transform1 * mContacts[i]->getLocalPointOnBody1()); - mContacts[i]->setWorldPointOnBody2(transform2 * mContacts[i]->getLocalPointOnBody2()); - mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); - } - - // Remove the contacts that don't represent very well the persistent contact - for (int i=mNbContacts-1; i>=0; i--) { - assert(i>= 0 && i < mNbContacts); - - // Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore) - if (mContacts[i]->getPenetrationDepth() <= 0.0) { - removeContact(i); - } - else { - // Compute the distance of the two contact points in the place orthogonal to the contact normal - Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth(); - Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; - - // If the orthogonal distance is larger than the valid distance threshold, we remove the contact - if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) { - removeContact(i); - } - } - } -} - -// Return the index of the contact with the larger penetration depth. This -// corresponding contact will be kept in the cache. The method returns -1 is -// the new contact is the deepest. -int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const { - assert(mNbContacts == MAX_CONTACTS_IN_CACHE); - int indexMaxPenetrationDepth = -1; - decimal maxPenetrationDepth = newContact->getPenetrationDepth(); - - // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContacts[i]->getPenetrationDepth(); - indexMaxPenetrationDepth = i; - } - } - - // Return the index of largest penetration depth - return indexMaxPenetrationDepth; -} - -// Return the index that will be removed. The index of the contact with the larger penetration -// depth is given as a parameter. This contact won't be removed. Given this contact, we compute -// the different area and we want to keep the contacts with the largest area. The new point is also -// kept. -int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { - assert(mNbContacts == MAX_CONTACTS_IN_CACHE); - decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint - decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint - decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint - decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint - - if (indexMaxPenetration != 0) { - // Compute the area - Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area0 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 1) { - // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area1 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 2) { - // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area2 = crossProduct.lengthSquare(); - } - if (indexMaxPenetration != 3) { - // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[2]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); - Vector3 crossProduct = vector1.cross(vector2); - area3 = crossProduct.lengthSquare(); - } - - // Return the index of the contact to remove - return getMaxArea(area0, area1, area2, area3); -} - -// Return the index of maximum area -int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { - if (area0 < area1) { - if (area1 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area1 < area3) return 3; - else return 1; - } - } - else { - if (area0 < area2) { - if (area2 < area3) return 3; - else return 2; - } - else { - if (area0 < area3) return 3; - else return 0; - } - } -} - -// Clear the cache -void PersistentContactCache::clear() { - for (uint i=0; iContact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[i]); - } - mNbContacts = 0; -} \ No newline at end of file diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h deleted file mode 100644 index 0419ad71..00000000 --- a/src/engine/PersistentContactCache.h +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef PERSISTENT_CONTACT_CACHE_H -#define PERSISTENT_CONTACT_CACHE_H - -// Libraries -#include -#include "../body/Body.h" -#include "../constraint/Contact.h" - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Constants -const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache - - -/* ------------------------------------------------------------------- - Class PersistentContactCache : - This class represents a cache of at most 4 contact points between - two given bodies. The contacts between two bodies are added one - after the other in the cache. When the cache is full, we have - to remove one point. The idea is to keep the point with the deepest - penetration depth and also to keep the points producing the larger - area (for a more stable contact manifold). The new added point is - always kept. This kind of persistent cache has been explained for - instance in the presentation from Erin Catto about contact manifolds - at the GDC 2007 conference. - ------------------------------------------------------------------- -*/ -class PersistentContactCache { - - private: - - // -------------------- Attributes -------------------- // - - // Pointer to the first body - Body* const mBody1; - - // Pointer to the second body - Body* const mBody2; - - // Contacts in the cache - Contact* mContacts[MAX_CONTACTS_IN_CACHE]; - - // Number of contacts in the cache - uint mNbContacts; - - // Reference to the memory pool with the contacts - MemoryPool& mMemoryPoolContacts; - - // -------------------- Methods -------------------- // - - // Private copy-constructor - PersistentContactCache(const PersistentContactCache& persistentContactCache); - - // Private assignment operator - PersistentContactCache& operator=(const PersistentContactCache& persistentContactCache); - - // Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; - - // Return the index of the contact with the larger penetration depth - int getIndexOfDeepestPenetration(Contact* newContact) const; - - // Return the index that will be removed - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; - - // Remove a contact from the cache - void removeContact(int index); - - // Return true if two vectors are approximatively equal - bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; - - public: - - // -------------------- Methods -------------------- // - - // Constructor - PersistentContactCache(Body* const mBody1, Body* const mBody2, - MemoryPool& mMemoryPoolContacts); - - // Destructor - ~PersistentContactCache(); - - // Add a contact - void addContact(Contact* contact); - - // Update the contact cache - void update(const Transform& transform1, const Transform& transform2); - - // Clear the cache - void clear(); - - // Return the number of contacts in the cache - uint getNbContacts() const; - - // Return a contact of the cache - Contact* getContact(uint index) const; -}; - -// Return the number of contacts in the cache -inline uint PersistentContactCache::getNbContacts() const { - return mNbContacts; -} - -// Return a contact of the cache -inline Contact* PersistentContactCache::getContact(uint index) const { - assert(index >= 0 && index < mNbContacts); - return mContacts[index]; -} - -// Return true if two vectors are approximatively equal -inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, - const Vector3& vector2) const { - const decimal epsilon = 0.1; - return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && - approxEqual(vector1.getY(), vector2.getY(), epsilon) && - approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); -} - -} -#endif - diff --git a/src/engine/Timer.h b/src/engine/Timer.h index aab7160a..474481ac 100644 --- a/src/engine/Timer.h +++ b/src/engine/Timer.h @@ -153,7 +153,7 @@ inline void Timer::start() { LARGE_INTEGER ticks; QueryPerformanceFrequency(&ticksPerSecond); QueryPerformanceCounter(&ticks); - lastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart); + mLastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart); #else // Initialize the lastUpdateTime with the current time in seconds timeval timeValue; 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, diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 437448e8..c44e3820 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -26,7 +26,6 @@ // Libraries #include "Vector3.h" #include -#include #include // Namespaces @@ -97,3 +96,27 @@ Vector3 Vector3::getOneOrthogonalVector() const { //assert(vector1.isUnit()); return vector1; } + +// Return one unit orthogonal vector of the current vector +Vector3 Vector3::getOneUnitOrthogonalVector() const { + assert(!this->isZero()); + + decimal x = mValues[0]; + decimal y = mValues[1]; + decimal z = mValues[2]; + + // Get the minimum element of the vector + Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); + int minElement = vectorAbs.getMinAxis(); + + if (minElement == 0) { + return Vector3(0.0, -z, y) / sqrt(y*y + z*z); + } + else if (minElement == 1) { + return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); + } + else { + return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); + } + +} diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 7cce12d5..bc549a08 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include "mathematics_functions.h" #include "../decimal.h" @@ -95,6 +96,9 @@ class Vector3 { // Return the corresponding unit vector Vector3 getUnit() const; + // Return one unit orthogonal vector of the current vector + Vector3 getOneUnitOrthogonalVector() const; + // Return true if the vector is unit and false otherwise bool isUnit() const; @@ -110,6 +114,9 @@ class Vector3 { // Cross product of two vectors Vector3 cross(const Vector3& vector) const; + // Normalize the vector + void normalize(); + // Return the corresponding absolute value vector Vector3 getAbsoluteVector() const; @@ -137,6 +144,9 @@ class Vector3 { // Overloaded operator for multiplication with a number with assignment Vector3& operator*=(decimal number); + // Overloaded operator for division by a number with assignment + Vector3& operator/=(decimal number); + // Overloaded operator for value access decimal& operator[] (int index); @@ -153,6 +163,7 @@ class Vector3 { friend Vector3 operator-(const Vector3& vector); friend Vector3 operator*(const Vector3& vector, decimal number); friend Vector3 operator*(decimal number, const Vector3& vector); + friend Vector3 operator/(const Vector3& vector, decimal number); }; // Get the x component of the vector @@ -217,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const { mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]); } +// Normalize the vector +inline void Vector3::normalize() { + decimal l = length(); + assert(l != 0.0); + mValues[0] /= l; + mValues[1] /= l; + mValues[2] /= l; +} + // Return the corresponding absolute value vector inline Vector3 Vector3::getAbsoluteVector() const { return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2])); @@ -283,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) { return *this; } +// Overloaded operator for division by a number with assignment +inline Vector3& Vector3::operator/=(decimal number) { + mValues[0] /= number; + mValues[1] /= number; + mValues[2] /= number; + return *this; +} + // Overloaded operator for value access inline decimal& Vector3::operator[] (int index) { return mValues[index]; @@ -313,6 +341,11 @@ inline Vector3 operator*(const Vector3& vector, decimal number) { return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]); } +// Overloaded operator for division by a number +inline Vector3 operator/(const Vector3& vector, decimal number) { + return Vector3(vector.mValues[0] / number, vector.mValues[1] / number, vector.mValues[2] / number); +} + // Overloaded operator for multiplication with a number inline Vector3 operator*(decimal number, const Vector3& vector) { return vector * number;