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

Implement the non-linear-gauss-seidel position error correction

Browse Source
This commit is contained in:
Daniel Chappuis 2013-06-26 22:28:31 +02:00
parent 1c0726d9d6
commit ce0078c2a9
24 changed files with 1429 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/body/CollisionBody.h
View File

@ -239,8 +239,6 @@ inline void CollisionBody::updateOldTransform() {
// Update the rigid body in order to reflect a change in the body state // Update the rigid body in order to reflect a change in the body state
inline void CollisionBody::updateAABB() { inline void CollisionBody::updateAABB() {
// TODO : An AABB should not be updated every frame but only if the body has moved
// Update the AABB // Update the AABB
mCollisionShape->updateAABB(mAabb, mTransform); mCollisionShape->updateAABB(mAabb, mTransform);
} }

15
src/collision/BroadPhasePair.h
View File

@ -60,6 +60,9 @@ struct BroadPhasePair {
/// Destructor /// Destructor
~BroadPhasePair(); ~BroadPhasePair();
/// Return the pair of bodies index
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
/// Return the pair of bodies index /// Return the pair of bodies index
bodyindexpair getBodiesIndexPair() const; bodyindexpair getBodiesIndexPair() const;
@ -77,16 +80,22 @@ struct BroadPhasePair {
}; };
// Return the pair of bodies index // Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const { inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1,
CollisionBody* body2) {
// Construct the pair of body index // Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ? bodyindexpair indexPair = body1->getID() < body2->getID() ?
std::make_pair(body1->getID(), body2->getID()) : std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID()); std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second); assert(indexPair.first != indexPair.second);
return indexPair; return indexPair;
} }
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
return computeBodiesIndexPair(body1, body2);
}
// Smaller than operator // Smaller than operator
inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const { inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const {
return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2)); return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2));

3
src/collision/CollisionDetection.cpp
View File

@ -109,6 +109,9 @@ void CollisionDetection::computeNarrowPhase() {
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
mWorld->updateOverlappingPair(pair); mWorld->updateOverlappingPair(pair);
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue;
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm( NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(

23
src/collision/CollisionDetection.h
View File

@ -79,6 +79,9 @@ class CollisionDetection {
/// Narrow-phase Sphere vs Sphere algorithm /// Narrow-phase Sphere vs Sphere algorithm
SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm; SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm;
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
@ -113,6 +116,12 @@ class CollisionDetection {
/// Remove a body from the collision detection /// Remove a body from the collision detection
void removeBody(CollisionBody* body); void removeBody(CollisionBody* body);
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Remove a pair of bodies that cannot collide with each other
void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2);
/// Compute the collision detection /// Compute the collision detection
void computeCollisionDetection(); void computeCollisionDetection();
@ -148,7 +157,19 @@ inline void CollisionDetection::removeBody(CollisionBody* body) {
// Remove the body from the broad-phase // Remove the body from the broad-phase
mBroadPhaseAlgorithm->removeObject(body); mBroadPhaseAlgorithm->removeObject(body);
} }
// Add a pair of bodies that cannot collide with each other
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2));
}
// Remove a pair of bodies that cannot collide with each other
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2));
}
} }

2
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
int minAxis = d.getAbsoluteVector().getMinAxis(); int minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60) // Compute sin(60)
const decimal sin60 = sqrt(3.0) * decimal(0.5); const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors // Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3 // v2 and v3

15
src/configuration.h
View File

@ -54,15 +54,14 @@ typedef std::pair<bodyindex, bodyindex> bodyindexpair;
// ------------------- Enumerations ------------------- // // ------------------- Enumerations ------------------- //
/// Position correction technique used in the constraint solver (for joints). /// Position correction technique used in the constraint solver (for joints).
/// BAUMGARTE : Faster but can be innacurate in some situations. This is the option /// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
/// used by default. /// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise.
enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL}; enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
/// Position correction technique used in the contact solver (for contacts) /// Position correction technique used in the contact solver (for contacts)
/// BAUMGARTE : Faster but can be innacurate and can lead to unexpected bounciness /// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
/// in some situations (due to error correction factor being added to /// in some situations (due to error correction factor being added to
/// the bodies momentum). /// the bodies momentum).
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the /// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
/// bodies momentum. This is the option used by default. /// bodies momentum. This is the option used by default.
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
@ -93,7 +92,7 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
/// True if the deactivation (sleeping) of inactive bodies is enabled /// True if the deactivation (sleeping) of inactive bodies is enabled
const bool DEACTIVATION_ENABLED = true; const bool DEACTIVATION_ENABLED = true;
///Object margin for collision detection in cm (For GJK-EPA Algorithm) /// Object margin for collision detection in cm (For GJK-EPA Algorithm)
const decimal OBJECT_MARGIN = decimal(0.04); const decimal OBJECT_MARGIN = decimal(0.04);
/// Distance threshold for two contact points for a valid persistent contact (in meters) /// Distance threshold for two contact points for a valid persistent contact (in meters)
@ -106,7 +105,7 @@ const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0);
const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15;
/// Number of iterations when solving the position constraints of the Sequential Impulse technique /// Number of iterations when solving the position constraints of the Sequential Impulse technique
const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 3; // TODO : Maybe we can use less iterations here const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
} }

152
src/constraint/BallAndSocketJoint.cpp
View File

@ -27,7 +27,6 @@
#include "BallAndSocketJoint.h" #include "BallAndSocketJoint.h"
#include "../engine/ConstraintSolver.h" #include "../engine/ConstraintSolver.h"
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition // Static variables definition
@ -61,8 +60,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -84,10 +83,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
// Compute the inverse mass matrix K^-1 // Compute the inverse mass matrix K^-1
@ -120,26 +119,29 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
const Vector3 linearImpulseBody2 = mImpulse;
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = mImpulse;
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
@ -152,11 +154,9 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse();
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// Compute J*v // Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
@ -165,25 +165,117 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
mImpulse += deltaLambda; mImpulse += deltaLambda;
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
// Solve the position constraint // Solve the position constraint (for position error correction)
void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += inverseMassBody1;
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += inverseMassBody2;
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrix.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrix = massMatrix.getInverse();
}
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + mR2World - x1 - mR1World);
// Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the
// right-hand side vector but instead use a method to directly solve the linear system.
const Vector3 lambda = mInverseMassMatrix * (-constraintError);
// Apply the impulse to the bodies of the joint (directly update the position/orientation)
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 linearImpulseBody2 = lambda;
const Vector3 angularImpulseBody2 = -lambda.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
} }

8
src/constraint/BallAndSocketJoint.h
View File

@ -81,6 +81,12 @@ class BallAndSocketJoint : public Constraint {
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Bias vector for the constraint /// Bias vector for the constraint
Vector3 mBiasVector; Vector3 mBiasVector;
@ -112,7 +118,7 @@ class BallAndSocketJoint : public Constraint {
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };

3
src/constraint/Constraint.cpp
View File

@ -32,7 +32,8 @@ using namespace reactphysics3d;
Constraint::Constraint(const ConstraintInfo& constraintInfo) Constraint::Constraint(const ConstraintInfo& constraintInfo)
:mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true), :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
mType(constraintInfo.type), mType(constraintInfo.type),
mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique) { mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique),
mIsCollisionEnabled(constraintInfo.isCollisionEnabled){
assert(mBody1 != NULL); assert(mBody1 != NULL);
assert(mBody2 != NULL); assert(mBody2 != NULL);

22
src/constraint/Constraint.h
View File

@ -59,6 +59,9 @@ struct ConstraintInfo {
/// Type of the constraint /// Type of the constraint
ConstraintType type; ConstraintType type;
/// True if the two bodies of the constraint are allowed to collide with each other
bool isCollisionEnabled;
/// Position correction technique used for the constraint (used for joints). /// Position correction technique used for the constraint (used for joints).
/// By default, the BAUMGARTE technique is used /// By default, the BAUMGARTE technique is used
JointsPositionCorrectionTechnique positionCorrectionTechnique; JointsPositionCorrectionTechnique positionCorrectionTechnique;
@ -66,12 +69,14 @@ struct ConstraintInfo {
/// Constructor /// Constructor
ConstraintInfo(ConstraintType constraintType) ConstraintInfo(ConstraintType constraintType)
: body1(NULL), body2(NULL), type(constraintType), : body1(NULL), body2(NULL), type(constraintType),
positionCorrectionTechnique(BAUMGARTE_JOINTS) {} positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) {}
/// Constructor /// Constructor
ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType) ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
: body1(rigidBody1), body2(rigidBody2), type(constraintType), : body1(rigidBody1), body2(rigidBody2), type(constraintType),
positionCorrectionTechnique(BAUMGARTE_JOINTS) { positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
isCollisionEnabled(true) {
} }
/// Destructor /// Destructor
@ -113,6 +118,9 @@ class Constraint {
/// Position correction technique used for the constraint (used for joints) /// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique; JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
/// True if the two bodies of the constraint are allowed to collide with each other
bool mIsCollisionEnabled;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Private copy-constructor /// Private copy-constructor
@ -143,6 +151,9 @@ class Constraint {
/// Return the type of the constraint /// Return the type of the constraint
ConstraintType getType() const; ConstraintType getType() const;
/// Return true if the collision between the two bodies of the constraint is enabled
bool isCollisionEnabled() const;
/// Return the number of bytes used by the constraint /// Return the number of bytes used by the constraint
virtual size_t getSizeInBytes() const = 0; virtual size_t getSizeInBytes() const = 0;
@ -177,7 +188,12 @@ inline bool Constraint::isActive() const {
// Return the type of the constraint // Return the type of the constraint
inline ConstraintType Constraint::getType() const { inline ConstraintType Constraint::getType() const {
return mType; return mType;
} }
// Return true if the collision between the two bodies of the constraint is enabled
inline bool Constraint::isCollisionEnabled() const {
return mIsCollisionEnabled;
}
} }

243
src/constraint/FixedJoint.cpp
View File

@ -68,8 +68,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -79,7 +79,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0; decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse(); inverseMassBodies += mBody1->getMassInverse();
@ -91,10 +91,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
// Compute the inverse mass matrix K^-1 for the 3 translation constraints // Compute the inverse mass matrix K^-1 for the 3 translation constraints
@ -114,10 +114,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero(); mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotation += I1; mInverseMassMatrixRotation += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation += I2; mInverseMassMatrixRotation += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
@ -150,32 +150,36 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
angularImpulseBody2 += mImpulseRotation;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2;
}
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
}
} }
// Solve the velocity constraint // Solve the velocity constraint
@ -187,11 +191,9 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse();
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -203,20 +205,25 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBiasTranslation); (-JvTranslation - mBiasTranslation);
mImpulseTranslation += deltaLambda; mImpulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
Vector3 linearImpulseBody2 = deltaLambda;
Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambda;
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambda;
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -228,21 +235,163 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
mImpulseRotation += deltaLambda2; mImpulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints if (mBody1->getIsMotionEnabled()) {
angularImpulseBody1 = -deltaLambda2;
angularImpulseBody2 = deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
}
}
// Solve the position constraint (for position error correction)
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint // Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 angularImpulseBody2 = lambdaRotation;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
} }
} }
// Solve the position constraint
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
}

8
src/constraint/FixedJoint.h
View File

@ -81,6 +81,12 @@ class FixedJoint : public Constraint {
/// Vector from center of body 2 to anchor point in world-space /// Vector from center of body 2 to anchor point in world-space
Vector3 mR2World; Vector3 mR2World;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Accumulated impulse for the 3 translation constraints /// Accumulated impulse for the 3 translation constraints
Vector3 mImpulseTranslation; Vector3 mImpulseTranslation;
@ -124,7 +130,7 @@ class FixedJoint : public Constraint {
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };

446
src/constraint/HingeJoint.cpp
View File

@ -40,7 +40,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce) { mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI); assert(mUpperLimit >= 0 && mUpperLimit <= 2.0 * PI);
@ -83,8 +83,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1; mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -107,8 +107,6 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mImpulseUpperLimit = 0.0; mImpulseUpperLimit = 0.0;
} }
decimal testAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
mA1 = orientationBody1 * mHingeLocalAxisBody1; mA1 = orientationBody1 * mHingeLocalAxisBody1;
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
@ -135,10 +133,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies); 0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
} }
mInverseMassMatrixTranslation.setToZero(); mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
@ -158,12 +156,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Vector3 I2B2CrossA1(0, 0, 0); Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0); Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
I1B2CrossA1 = I1 * mB2CrossA1; I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = I1 * mC2CrossA1; I1C2CrossA1 = mI1 * mC2CrossA1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
I2B2CrossA1 = I2 * mB2CrossA1; I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = I2 * mC2CrossA1; I2C2CrossA1 = mI2 * mC2CrossA1;
} }
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1); mB2CrossA1.dot(I2B2CrossA1);
@ -201,10 +199,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0; mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(I1 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(I2 * mA1); mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
} }
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
@ -235,38 +233,53 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints // Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
angularImpulseBody1 += rotationImpulse;
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints // Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1;
angularImpulseBody1 += limitsImpulse;
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint // Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mImpulseMotor * mA1; const Vector3 motorImpulse = -mImpulseMotor * mA1;
angularImpulseBody1 += motorImpulse;
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody1 = -mImpulseTranslation;
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody1 += motorImpulse;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 translation constraints
Vector3 linearImpulseBody2 = mImpulseTranslation;
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
@ -282,8 +295,6 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse();
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -295,20 +306,25 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
(-JvTranslation - mBTranslation); (-JvTranslation - mBTranslation);
mImpulseTranslation += deltaLambdaTranslation; mImpulseTranslation += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda
Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
Vector3 linearImpulseBody2 = deltaLambdaTranslation;
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
const Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 linearImpulseBody2 = deltaLambdaTranslation;
const Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -321,16 +337,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambdaRotation; mImpulseRotation += deltaLambdaRotation;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - mC2CrossA1 * deltaLambdaRotation.y;
angularImpulseBody2 = -angularImpulseBody1;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
const Vector3 angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
mC2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
} }
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -344,21 +367,26 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
const decimal JvLowerLimit = (w2 - w1).dot(mA1); const decimal JvLowerLimit = (w2 - w1).dot(mA1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit - mBLowerLimit); decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
decimal lambdaTemp = mImpulseLowerLimit; decimal lambdaTemp = mImpulseLowerLimit;
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
const Vector3 angularImpulseBody2 = -angularImpulseBody1;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
} }
} }
@ -374,16 +402,21 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
const Vector3 angularImpulseBody2 = -angularImpulseBody1;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
} }
} }
} }
@ -396,29 +429,288 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
const decimal JvMotor = mA1.dot(w1 - w2); const decimal JvMotor = mA1.dot(w1 - w2);
// Compute the Lagrange multiplier lambda for the motor // Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
decimal lambdaTemp = mImpulseMotor; decimal lambdaTemp = mImpulseMotor;
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
const Vector3 angularImpulseBody2 = -angularImpulseBody1;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
} }
} }
} }
// Solve the position constraint // Solve the position constraint (for position error correction)
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
mR2World = q2 * mLocalAnchorPointBody2;
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mLowerLimit;
decimal upperLimitError = mUpperLimit - hingeAngle;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
// Compute vectors needed in the Jacobian
mA1 = q1 * mHingeLocalAxisBody1;
Vector3 a2 = q2 * mHingeLocalAxisBody2;
mA1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mB2CrossA1 = b2.cross(mA1);
mC2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = 0.0;
if (mBody1->getIsMotionEnabled()) {
inverseMassBodies += mBody1->getMassInverse();
}
if (mBody2->getIsMotionEnabled()) {
inverseMassBodies += mBody2->getMassInverse();
}
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies);
if (mBody1->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
}
if (mBody2->getIsMotionEnabled()) {
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
}
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
// Compute the pseudo velocity
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
Vector3 linearImpulseBody2 = lambdaTranslation;
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
// Compute the pseudo velocity
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1(0, 0, 0);
Vector3 I1C2CrossA1(0, 0, 0);
Vector3 I2B2CrossA1(0, 0, 0);
Vector3 I2C2CrossA1(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
I1B2CrossA1 = mI1 * mB2CrossA1;
I1C2CrossA1 = mI1 * mC2CrossA1;
}
if (mBody2->getIsMotionEnabled()) {
I2B2CrossA1 = mI2 * mB2CrossA1;
I2C2CrossA1 = mI2 * mC2CrossA1;
}
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
mB2CrossA1.dot(I2B2CrossA1);
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
mB2CrossA1.dot(I2C2CrossA1);
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
mC2CrossA1.dot(I2B2CrossA1);
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x -
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse
const Vector3 angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x +
mC2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimitMotor = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
}
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
}
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
// Compute the pseudo velocity
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
// Compute the pseudo velocity
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
} }
@ -492,13 +784,13 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
} }
} }
// Set the maximum motor force // Set the maximum motor torque
void HingeJoint::setMaxMotorForce(decimal maxMotorForce) { void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
if (maxMotorForce != mMaxMotorForce) { if (maxMotorTorque != mMaxMotorTorque) {
assert(mMaxMotorForce >= 0.0); assert(mMaxMotorTorque >= 0.0);
mMaxMotorForce = maxMotorForce; mMaxMotorTorque = maxMotorTorque;
// TODO : Wake up the bodies of the joint here when sleeping is implemented // TODO : Wake up the bodies of the joint here when sleeping is implemented
} }

53
src/constraint/HingeJoint.h
View File

@ -66,8 +66,9 @@ struct HingeJointInfo : public ConstraintInfo {
/// Motor speed (in radian/second) /// Motor speed (in radian/second)
decimal motorSpeed; decimal motorSpeed;
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed /// Maximum motor torque (in Newtons * meters) that can be applied to reach
decimal maxMotorForce; /// to desired motor speed
decimal maxMotorTorque;
/// Constructor without limits and without motor /// Constructor without limits and without motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
@ -76,8 +77,8 @@ struct HingeJointInfo : public ConstraintInfo {
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0), isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
maxMotorForce(0) {} motorSpeed(0), maxMotorTorque(0) {}
/// Constructor with limits but without motor /// Constructor with limits but without motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
@ -88,20 +89,21 @@ struct HingeJointInfo : public ConstraintInfo {
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorForce(0) {} maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
maxMotorTorque(0) {}
/// Constructor with limits and motor /// Constructor with limits and motor
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
const Vector3& initAnchorPointWorldSpace, const Vector3& initAnchorPointWorldSpace,
const Vector3& initRotationAxisWorld, const Vector3& initRotationAxisWorld,
decimal initMinAngleLimit, decimal initMaxAngleLimit, decimal initMinAngleLimit, decimal initMaxAngleLimit,
decimal initMotorSpeed, decimal initMaxMotorForce) decimal initMotorSpeed, decimal initMaxMotorTorque)
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT), : ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
anchorPointWorldSpace(initAnchorPointWorldSpace), anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true), rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
isMotorEnabled(false), minAngleLimit(initMinAngleLimit), isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed), maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
maxMotorForce(initMaxMotorForce) {} maxMotorTorque(initMaxMotorTorque) {}
}; };
// Class HingeJoint // Class HingeJoint
@ -132,6 +134,12 @@ class HingeJoint : public Constraint {
/// Hinge rotation axis (in local-space coordiantes of body 2) /// Hinge rotation axis (in local-space coordiantes of body 2)
Vector3 mHingeLocalAxisBody2; Vector3 mHingeLocalAxisBody2;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Hinge rotation axis (in world-space coordinates) computed from body 1 /// Hinge rotation axis (in world-space coordinates) computed from body 1
Vector3 mA1; Vector3 mA1;
@ -210,15 +218,16 @@ class HingeJoint : public Constraint {
/// Motor speed /// Motor speed
decimal mMotorSpeed; decimal mMotorSpeed;
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce; decimal mMaxMotorTorque;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi] /// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi]
decimal computeNormalizedAngle(decimal angle) const; decimal computeNormalizedAngle(decimal angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an /// Given an "inputAngle" in the range [-pi, pi], this method returns an
@ -271,14 +280,14 @@ class HingeJoint : public Constraint {
/// Set the motor speed /// Set the motor speed
void setMotorSpeed(decimal motorSpeed); void setMotorSpeed(decimal motorSpeed);
/// Return the maximum motor force /// Return the maximum motor torque
decimal getMaxMotorForce() const; decimal getMaxMotorTorque() const;
/// Set the maximum motor force /// Set the maximum motor torque
void setMaxMotorForce(decimal maxMotorForce); void setMaxMotorTorque(decimal maxMotorTorque);
/// Return the intensity of the current force applied for the joint motor /// Return the intensity of the current torque applied for the joint motor
decimal getMotorForce(decimal timeStep) const; decimal getMotorTorque(decimal timeStep) const;
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
@ -292,7 +301,7 @@ class HingeJoint : public Constraint {
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };
@ -321,13 +330,13 @@ inline decimal HingeJoint::getMotorSpeed() const {
return mMotorSpeed; return mMotorSpeed;
} }
// Return the maximum motor force // Return the maximum motor torque
inline decimal HingeJoint::getMaxMotorForce() const { inline decimal HingeJoint::getMaxMotorTorque() const {
return mMaxMotorForce; return mMaxMotorTorque;
} }
// Return the intensity of the current force applied for the joint motor // Return the intensity of the current torque applied for the joint motor
inline decimal HingeJoint::getMotorForce(decimal timeStep) const { inline decimal HingeJoint::getMotorTorque(decimal timeStep) const {
return mImpulseMotor / timeStep; return mImpulseMotor / timeStep;
} }

482
src/constraint/SliderJoint.cpp
View File

@ -36,9 +36,10 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit), mLowerLimit(jointInfo.minTranslationLimit),
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false),
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){ mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed),
mMaxMotorForce(jointInfo.maxMotorForce){
assert(mUpperLimit >= 0.0); assert(mUpperLimit >= 0.0);
assert(mLowerLimit <= 0.0); assert(mLowerLimit <= 0.0);
@ -81,8 +82,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); mI1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = orientationBody1 * mLocalAnchorPointBody1; mR1 = orientationBody1 * mLocalAnchorPointBody1;
@ -130,13 +131,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
Vector3 I2R2CrossN2(0, 0, 0); Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse(); sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1; I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2; I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse(); sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = I2 * mR2CrossN1; I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = I2 * mR2CrossN2; I2R2CrossN2 = mI2 * mR2CrossN2;
} }
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); mR2CrossN1.dot(I2R2CrossN1);
@ -165,10 +166,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// contraints (3x3 matrix) // contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero(); mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += I1; mInverseMassMatrixRotationConstraint += mI1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += I2; mInverseMassMatrixRotationConstraint += mI2;
} }
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
@ -189,11 +190,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mInverseMassMatrixLimit = 0.0; mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() + mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis); mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() + mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
} }
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
@ -246,42 +247,55 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->getMassInverse(); const decimal inverseMassBody1 = mBody1->getMassInverse();
const decimal inverseMassBody2 = mBody2->getMassInverse(); const decimal inverseMassBody2 = mBody2->getMassInverse();
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
Vector3 linearImpulseBody2 = -linearImpulseBody1;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints // Compute the impulse P=J^T * lambda for the lower and upper limits constraints
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint // Compute the impulse P=J^T * lambda for the motor constraint
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
linearImpulseBody1 += impulseMotor;
linearImpulseBody2 += -impulseMotor;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 += -mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody1 += impulseMotor;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody2 += mImpulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
// Compute the impulse P=J^T * lambda for the motor constraint
linearImpulseBody2 += -impulseMotor;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
@ -297,8 +311,6 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse();
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -313,20 +325,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += deltaLambda; mImpulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for the 2 translation constraints
Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -mR1PlusUCrossN2 * deltaLambda.y;
Vector3 linearImpulseBody2 = -linearImpulseBody1;
Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
@ -338,16 +356,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambda2; mImpulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
angularImpulseBody1 = -deltaLambda2;
angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
w1 += I1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body
w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
w2 += I2 * angularImpulseBody2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body
w2 += mI2 * angularImpulseBody2;
} }
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -367,20 +390,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
@ -397,20 +425,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += I1 * angularImpulseBody1; w1 += mI1 * angularImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
w2 += I2 * angularImpulseBody2; w2 += mI2 * angularImpulseBody2;
} }
} }
} }
@ -429,23 +462,292 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mImpulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
// Apply the impulse to the bodies of the joint
if (mBody1->getIsMotionEnabled()) { if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
} }
if (mBody2->getIsMotionEnabled()) { if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the motor
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld;
// Apply the impulse to the body
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
} }
} }
} }
// Solve the position constraint // Solve the position constraint (for position error correction)
void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->getMassInverse();
decimal inverseMassBody2 = mBody2->getMassInverse();
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point
mR1 = q1 * mLocalAnchorPointBody1;
mR2 = q2 * mLocalAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
// Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = q1 * mSliderAxisBody1;
mSliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector();
mN2 = mSliderAxisWorld.cross(mN1);
// Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
mIsLowerLimitViolated = lowerLimitError <= 0;
mIsUpperLimitViolated = upperLimitError <= 0;
// Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1);
mR2CrossN2 = mR2.cross(mN2);
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld);
const Vector3 r1PlusU = mR1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1);
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld);
// --------------- Translation Constraints --------------- //
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = 0.0;
Vector3 I1R1PlusUCrossN1(0, 0, 0);
Vector3 I1R1PlusUCrossN2(0, 0, 0);
Vector3 I2R2CrossN1(0, 0, 0);
Vector3 I2R2CrossN2(0, 0, 0);
if (mBody1->getIsMotionEnabled()) {
sumInverseMass += mBody1->getMassInverse();
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
}
if (mBody2->getIsMotionEnabled()) {
sumInverseMass += mBody2->getMassInverse();
I2R2CrossN1 = mI2 * mR2CrossN1;
I2R2CrossN2 = mI2 * mR2CrossN2;
}
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
// Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(mN1), u.dot(mN2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 2 translation constraints
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y;
const Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x +
mR2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint.setToZero();
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI1;
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint += mI2;
}
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
const Vector3 angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = 0.0;
if (mBody1->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody1->getMassInverse() +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
}
if (mBody2->getIsMotionEnabled()) {
mInverseMassMatrixLimit += mBody2->getMassInverse() +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
}
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
}
// If the lower limit is violated
if (mIsLowerLimitViolated) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the lower limit constraint
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
// If the upper limit is violated
if (mIsUpperLimitViolated) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError);
if (mBody1->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis;
// Apply the impulse to the body
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
}
if (mBody2->getIsMotionEnabled()) {
// Compute the impulse P=J^T * lambda for the upper limit constraint
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis;
// Apply the impulse to the body
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
// Update the body position/orientation
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
} }
// Enable/Disable the limits of the joint // Enable/Disable the limits of the joint
@ -469,6 +771,30 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
// TODO : Wake up the bodies of the joint here when sleeping is implemented // TODO : Wake up the bodies of the joint here when sleeping is implemented
} }
// Return the current translation value of the joint
decimal SliderJoint::getTranslation() const {
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& q1 = mBody1->getTransform().getOrientation();
const Quaternion& q2 = mBody2->getTransform().getOrientation();
// Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1;
const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2;
// Compute the vector u (difference between anchor points)
const Vector3 u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space
Vector3 sliderAxisWorld = q1 * mSliderAxisBody1;
sliderAxisWorld.normalize();
// Compute and return the translation value
return u.dot(sliderAxisWorld);
}
// Set the minimum translation limit // Set the minimum translation limit
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {

15
src/constraint/SliderJoint.h
View File

@ -64,7 +64,7 @@ struct SliderJointInfo : public ConstraintInfo {
/// Motor speed /// Motor speed
decimal motorSpeed; decimal motorSpeed;
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal maxMotorForce; decimal maxMotorForce;
/// Constructor without limits and without motor /// Constructor without limits and without motor
@ -129,6 +129,12 @@ class SliderJoint : public Constraint {
/// Slider axis (in local-space coordinates of body 1) /// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1; Vector3 mSliderAxisBody1;
/// Inertia tensor of body 1 (in world-space coordinates)
Matrix3x3 mI1;
/// Inertia tensor of body 2 (in world-space coordinates)
Matrix3x3 mI2;
/// Inverse of the initial orientation difference between the two bodies /// Inverse of the initial orientation difference between the two bodies
Quaternion mInitOrientationDifferenceInv; Quaternion mInitOrientationDifferenceInv;
@ -225,7 +231,7 @@ class SliderJoint : public Constraint {
/// Motor speed /// Motor speed
decimal mMotorSpeed; decimal mMotorSpeed;
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce; decimal mMaxMotorForce;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -255,6 +261,9 @@ class SliderJoint : public Constraint {
/// Enable/Disable the motor of the joint /// Enable/Disable the motor of the joint
void enableMotor(bool isMotorEnabled); void enableMotor(bool isMotorEnabled);
/// Return the current translation value of the joint
decimal getTranslation() const;
/// Return the minimum translation limit /// Return the minimum translation limit
decimal getMinTranslationLimit() const; decimal getMinTranslationLimit() const;
@ -294,7 +303,7 @@ class SliderJoint : public Constraint {
/// Solve the velocity constraint /// Solve the velocity constraint
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
/// Solve the position constraint /// Solve the position constraint (for position error correction)
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData); virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
}; };

19
src/engine/ConstraintSolver.cpp
View File

@ -33,14 +33,15 @@ using namespace reactphysics3d;
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints, ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities, std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities, std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex) const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mJoints(joints), mLinearVelocities(linearVelocities), : mJoints(joints), mLinearVelocities(linearVelocities),
mAngularVelocities(angularVelocities), mAngularVelocities(angularVelocities), mPositions(positions),
mOrientations(orientations),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
mIsNonLinearGaussSeidelPositionCorrectionActive(false), angularVelocities, positions, orientations, mapBodyToVelocityIndex){
mConstraintSolverData(linearVelocities,
angularVelocities, mapBodyToVelocityIndex){
} }
@ -94,10 +95,8 @@ void ConstraintSolver::solveVelocityConstraints() {
std::set<Constraint*>::iterator it; std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) { for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Solve the constraint // Solve the constraint
joint->solveVelocityConstraint(mConstraintSolverData); (*it)->solveVelocityConstraint(mConstraintSolverData);
} }
} }
@ -110,9 +109,7 @@ void ConstraintSolver::solvePositionConstraints() {
std::set<Constraint*>::iterator it; std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) { for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Solve the constraint // Solve the constraint
joint->solveVelocityConstraint(mConstraintSolverData); (*it)->solvePositionConstraint(mConstraintSolverData);
} }
} }

34
src/engine/ConstraintSolver.h
View File

@ -53,6 +53,12 @@ struct ConstraintSolverData {
/// Reference to the bodies angular velocities /// Reference to the bodies angular velocities
std::vector<Vector3>& angularVelocities; std::vector<Vector3>& angularVelocities;
/// Reference to the bodies positions
std::vector<Vector3>& positions;
/// Reference to the bodies orientations
std::vector<Quaternion>& orientations;
/// Reference to the map that associates rigid body to their index /// Reference to the map that associates rigid body to their index
/// in the constrained velocities array /// in the constrained velocities array
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
@ -63,9 +69,12 @@ struct ConstraintSolverData {
/// Constructor /// Constructor
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities, ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
std::vector<Vector3>& refAngularVelocities, std::vector<Vector3>& refAngularVelocities,
std::vector<Vector3>& refPositions,
std::vector<Quaternion>& refOrientations,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex) const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(refLinearVelocities), :linearVelocities(refLinearVelocities),
angularVelocities(refAngularVelocities), angularVelocities(refAngularVelocities),
positions(refPositions), orientations(refOrientations),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
} }
@ -161,6 +170,12 @@ class ConstraintSolver {
/// after solving the constraints) /// after solving the constraints)
std::vector<Vector3>& mAngularVelocities; std::vector<Vector3>& mAngularVelocities;
/// Reference to the array of bodies positions (for position error correction)
std::vector<Vector3>& mPositions;
/// Reference to the array of bodies orientations (for position error correction)
std::vector<Quaternion>& mOrientations;
/// Reference to the map that associates rigid body to their index in /// Reference to the map that associates rigid body to their index in
/// the constrained velocities array /// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex; const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@ -171,9 +186,6 @@ class ConstraintSolver {
/// True if the warm starting of the solver is active /// True if the warm starting of the solver is active
bool mIsWarmStartingActive; bool mIsWarmStartingActive;
/// True if the Non-Linear-Gauss-Seidel position correction technique is enabled
bool mIsNonLinearGaussSeidelPositionCorrectionActive;
/// Constraint solver data used to initialize and solve the constraints /// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData; ConstraintSolverData mConstraintSolverData;
@ -185,6 +197,8 @@ class ConstraintSolver {
ConstraintSolver(std::set<Constraint*>& joints, ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities, std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities, std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex); const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor /// Destructor
@ -204,16 +218,14 @@ class ConstraintSolver {
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
}; };
// Return true if the Non-Linear-Gauss-Seidel position correction technique is active // Return true if the body is in at least one constraint
inline bool ConstraintSolver::getIsNonLinearGaussSeidelPositionCorrectionActive() const { inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
return mIsNonLinearGaussSeidelPositionCorrectionActive; return mConstraintBodies.count(body) == 1;
}
// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
inline void ConstraintSolver::setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive) {
mIsNonLinearGaussSeidelPositionCorrectionActive = isActive;
} }
} }

10
src/engine/ContactSolver.h
View File

@ -442,6 +442,9 @@ class ContactSolver {
/// Clean up the constraint solver /// Clean up the constraint solver
void cleanup(); void cleanup();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
/// Activate or Deactivate the split impulses for contacts /// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive); void setIsSplitImpulseActive(bool isActive);
@ -469,6 +472,11 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
return mSplitAngularVelocities[indexBody]; return mSplitAngularVelocities[indexBody];
} }
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive;
}
// Activate or Deactivate the split impulses for contacts // Activate or Deactivate the split impulses for contacts
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive; mIsSplitImpulseActive = isActive;
@ -482,7 +490,7 @@ inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool
// Compute the collision restitution factor from the restitution factor of each body // Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1, inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
const RigidBody* body2) const { const RigidBody* body2) const {
decimal restitution1 = body1->getRestitution(); decimal restitution1 = body1->getRestitution();
decimal restitution2 = body2->getRestitution(); decimal restitution2 = body2->getRestitution();

135
src/engine/DynamicsWorld.cpp
View File

@ -40,6 +40,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex), mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mConstrainedPositions, mConstrainedOrientations,
mMapBodyToConstrainedVelocityIndex), mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
@ -102,18 +103,24 @@ void DynamicsWorld::update() {
// Integrate the velocities // Integrate the velocities
integrateRigidBodiesVelocities(); integrateRigidBodiesVelocities();
// Solve the contacts and constraints // Reset the movement boolean variable of each body to false
solveContactsAndConstraints(); resetBodiesMovementVariable();
// Update the timer // Update the timer
mTimer.nextStep(); mTimer.nextStep();
// Reset the movement boolean variable of each body to false // Solve the contacts and constraints
resetBodiesMovementVariable(); solveContactsAndConstraints();
// Integrate the position and orientation of each body // Integrate the position and orientation of each body
integrateRigidBodiesPositions(); integrateRigidBodiesPositions();
// Solve the position correction for constraints
solvePositionCorrection();
// Update the AABBs of the bodies
updateRigidBodiesAABB();
// Cleanup of the contact solver // Cleanup of the contact solver
mContactSolver.cleanup(); mContactSolver.cleanup();
@ -130,7 +137,7 @@ void DynamicsWorld::update() {
/// the sympletic Euler time stepping scheme. /// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() { void DynamicsWorld::integrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()"); PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
decimal dt = static_cast<decimal>(mTimer.getTimeStep()); decimal dt = static_cast<decimal>(mTimer.getTimeStep());
@ -144,9 +151,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
// If the body is allowed to move // If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) { if (rigidBody->getIsMotionEnabled()) {
// Update the old Transform of the body
rigidBody->updateOldTransform();
// Get the constrained velocity // Get the constrained velocity
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
@ -157,7 +161,9 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
rigidBody->setAngularVelocity(newAngVelocity); rigidBody->setAngularVelocity(newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used to update the position) // Add the split impulse velocity from Contact Solver (only used to update the position)
if (mContactSolver.isConstrainedBody(rigidBody)) { if (mContactSolver.isConstrainedBody(rigidBody) &&
mContactSolver.isSplitImpulseActive()) {
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
} }
@ -168,17 +174,30 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
// Compute the new position of the body // Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt; Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.x, Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
newAngVelocity.y, currentOrientation * decimal(0.5) * dt;
newAngVelocity.z, 0) *
currentOrientation * 0.5 * dt;
// Update the Transform of the body // Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit()); Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform); rigidBody->setTransform(newTransform);
}
}
}
// Update the AABBs of the bodies
void DynamicsWorld::updateRigidBodiesAABB() {
PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
// For each rigid body of the world
set<RigidBody*>::iterator it;
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
// If the body has moved
if ((*it)->getHasMoved()) {
// Update the AABB of the rigid body // Update the AABB of the rigid body
rigidBody->updateAABB(); (*it)->updateAABB();
} }
} }
} }
@ -210,6 +229,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
/// contact solver. /// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() { void DynamicsWorld::integrateRigidBodiesVelocities() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
// TODO : Use better memory allocation here // TODO : Use better memory allocation here
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0)); mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0)); mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
@ -231,6 +252,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Update the old Transform of the body
rigidBody->updateOldTransform();
} }
i++; i++;
@ -274,7 +298,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
// For each iteration of the velocity solver // For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) { for (uint i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the velocity constraints // Solve the constraints
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints(); if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
// Solve the contacts // Solve the contacts
@ -283,19 +307,60 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Cache the lambda values in order to use them in the next step // Cache the lambda values in order to use them in the next step
if (isContactsToSolve) mContactSolver.storeImpulses(); if (isContactsToSolve) mContactSolver.storeImpulses();
}
// ---------- Solve positions constraints (error correction) for joints only ---------- // // Solve the position error correction of the constraints
void DynamicsWorld::solvePositionCorrection() {
// TODO : Integrate the bodies positions here PROFILE("DynamicsWorld::solvePositionCorrection()");
// If the Non-Linear-Gauss-Seidel position correction is active // Do not continue if there is no constraints
if (mConstraintSolver.getIsNonLinearGaussSeidelPositionCorrectionActive()) { if (mJoints.empty()) return;
// For each iteration of the position (error correction) solver // ---------- Get the position/orientation of the rigid bodies ---------- //
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints // TODO : Use better memory allocation here
if (isConstraintsToSolve) mConstraintSolver.solvePositionConstraints(); mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
// Get the position/orientation of the rigid body
const Transform& transform = (*it)->getTransform();
mConstrainedPositions[index] = transform.getPosition();
mConstrainedOrientations[index]= transform.getOrientation();
}
}
// ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints();
}
// ---------- Update the position/orientation of the rigid bodies ---------- //
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
// Get the new position/orientation of the body
const Vector3& newPosition = mConstrainedPositions[index];
const Quaternion& newOrientation = mConstrainedOrientations[index];
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
(*it)->setTransform(newTransform);
} }
} }
} }
@ -365,7 +430,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
return rigidBody; return rigidBody;
} }
// Destroy a rigid body // Destroy a rigid body and all the joints which it belongs
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the body from the collision detection // Remove the body from the collision detection
@ -377,6 +442,14 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the collision shape from the world // Remove the collision shape from the world
removeCollisionShape(rigidBody->getCollisionShape()); removeCollisionShape(rigidBody->getCollisionShape());
// Destroy all the joints that contains the rigid body to be destroyed
bodyindex idToRemove = rigidBody->getID();
for (std::set<Constraint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) {
destroyJoint(*it);
}
}
// Call the destructor of the rigid body // Call the destructor of the rigid body
rigidBody->RigidBody::~RigidBody(); rigidBody->RigidBody::~RigidBody();
@ -440,6 +513,13 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
} }
} }
// If the collision between the two bodies of the constraint is disabled
if (!jointInfo.isCollisionEnabled) {
// Add the pair of bodies in the set of body pairs that cannot collide with each other
mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
}
// Add the joint into the world // Add the joint into the world
mJoints.insert(newJoint); mJoints.insert(newJoint);
@ -452,6 +532,13 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
assert(joint != NULL); assert(joint != NULL);
// If the collision between the two bodies of the constraint was disabled
if (!joint->isCollisionEnabled()) {
// Remove the pair of bodies from the set of body pairs that cannot collide with each other
mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
}
// Remove the joint from the world // Remove the joint from the world
mJoints.erase(joint); mJoints.erase(joint);

35
src/engine/DynamicsWorld.h
View File

@ -91,6 +91,12 @@ class DynamicsWorld : public CollisionWorld {
/// after solving the constraints) /// after solving the constraints)
std::vector<Vector3> mConstrainedAngularVelocities; std::vector<Vector3> mConstrainedAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
std::vector<Vector3> mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
std::vector<Quaternion> mConstrainedOrientations;
/// Map body to their index in the constrained velocities array /// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex; std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
@ -105,6 +111,9 @@ class DynamicsWorld : public CollisionWorld {
/// Integrate the positions and orientations of rigid bodies. /// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions(); void integrateRigidBodiesPositions();
/// Update the AABBs of the bodies
void updateRigidBodiesAABB();
/// Update the position and orientation of a body /// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity); Vector3 newAngVelocity);
@ -118,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
/// Solve the contacts and constraints /// Solve the contacts and constraints
void solveContactsAndConstraints(); void solveContactsAndConstraints();
/// Solve the position error correction of the constraints
void solvePositionCorrection();
/// Cleanup the constrained velocities array at each step /// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray(); void cleanupConstrainedVelocitiesArray();
@ -137,7 +149,8 @@ class DynamicsWorld : public CollisionWorld {
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair); virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact /// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair, const ContactPointInfo* contactInfo); virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo);
public : public :
@ -179,7 +192,7 @@ public :
const Matrix3x3& inertiaTensorLocal, const Matrix3x3& inertiaTensorLocal,
const CollisionShape& collisionShape); const CollisionShape& collisionShape);
/// Destroy a rigid body /// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody); void destroyRigidBody(RigidBody* rigidBody);
/// Create a joint between two bodies in the world and return a pointer to the new joint /// Create a joint between two bodies in the world and return a pointer to the new joint
@ -200,9 +213,15 @@ public :
/// Return the number of rigid bodies in the world /// Return the number of rigid bodies in the world
uint getNbRigidBodies() const; uint getNbRigidBodies() const;
/// Return the number of contact constraints in the world /// Return the number of joints in the world
uint getNbJoints() const;
/// Return the number of contact manifolds in the world
uint getNbContactManifolds() const; uint getNbContactManifolds() const;
/// Return the current physics time (in seconds)
long double getPhysicsTime() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world /// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
@ -302,6 +321,11 @@ inline uint DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size(); return mRigidBodies.size();
} }
/// Return the number of joints in the world
inline uint DynamicsWorld::getNbJoints() const {
return mJoints.size();
}
// Return an iterator to the beginning of the bodies of the physics world // Return an iterator to the beginning of the bodies of the physics world
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() { inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return mRigidBodies.begin(); return mRigidBodies.begin();
@ -317,6 +341,11 @@ inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size(); return mContactManifolds.size();
} }
/// Return the current physics time (in seconds)
inline long double DynamicsWorld::getPhysicsTime() const {
return mTimer.getPhysicsTime();
}
} }
#endif #endif

10
src/engine/Timer.h
View File

@ -47,7 +47,7 @@ namespace reactphysics3d {
// Class Timer // Class Timer
/** /**
* This class will take care of the time in the physics engine. It * This class will take care of the time in the physics engine. It
* uses fuunctions that depend on the current platform to get the * uses functions that depend on the current platform to get the
* current time. * current time.
*/ */
class Timer { class Timer {
@ -59,9 +59,6 @@ class Timer {
/// Timestep dt of the physics engine (timestep > 0.0) /// Timestep dt of the physics engine (timestep > 0.0)
double mTimeStep; double mTimeStep;
/// Current time of the physics engine
long double mTime;
/// Last time the timer has been updated /// Last time the timer has been updated
long double mLastUpdateTime; long double mLastUpdateTime;
@ -139,7 +136,7 @@ inline void Timer::setTimeStep(double timeStep) {
// Return the current time // Return the current time
inline long double Timer::getPhysicsTime() const { inline long double Timer::getPhysicsTime() const {
return mTime; return mLastUpdateTime;
} }
// Return if the timer is running // Return if the timer is running
@ -173,9 +170,6 @@ inline bool Timer::isPossibleToTakeStep() const {
inline void Timer::nextStep() { inline void Timer::nextStep() {
assert(mIsRunning); assert(mIsRunning);
// Update the current time of the physics engine
mTime += mTimeStep;
// Update the accumulator value // Update the accumulator value
mAccumulator -= mTimeStep; mAccumulator -= mTimeStep;
} }

24
src/mathematics/Quaternion.h
View File

@ -133,6 +133,12 @@ struct Quaternion {
/// Overloaded operator for the substraction /// Overloaded operator for the substraction
Quaternion operator-(const Quaternion& quaternion) const; Quaternion operator-(const Quaternion& quaternion) const;
/// Overloaded operator for addition with assignment
Quaternion& operator+=(const Quaternion& quaternion);
/// Overloaded operator for substraction with assignment
Quaternion& operator-=(const Quaternion& quaternion);
/// Overloaded operator for the multiplication with a constant /// Overloaded operator for the multiplication with a constant
Quaternion operator*(decimal nb) const; Quaternion operator*(decimal nb) const;
@ -272,6 +278,24 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w); return Quaternion(x - quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
} }
// Overloaded operator for addition with assignment
inline Quaternion& Quaternion::operator+=(const Quaternion& quaternion) {
x += quaternion.x;
y += quaternion.y;
z += quaternion.z;
w += quaternion.w;
return *this;
}
// Overloaded operator for substraction with assignment
inline Quaternion& Quaternion::operator-=(const Quaternion& quaternion) {
x -= quaternion.x;
y -= quaternion.y;
z -= quaternion.z;
w -= quaternion.w;
return *this;
}
// Overloaded operator for the multiplication with a constant // Overloaded operator for the multiplication with a constant
inline Quaternion Quaternion::operator*(decimal nb) const { inline Quaternion Quaternion::operator*(decimal nb) const {
return Quaternion(nb * x, nb * y, nb * z, nb * w); return Quaternion(nb * x, nb * y, nb * z, nb * w);

6
test/tests/mathematics/TestQuaternion.h
View File

@ -197,11 +197,17 @@ class TestQuaternion : public Test {
Quaternion quat1(4, 5, 2, 10); Quaternion quat1(4, 5, 2, 10);
Quaternion quat2(-2, 7, 8, 3); Quaternion quat2(-2, 7, 8, 3);
Quaternion test1 = quat1 + quat2; Quaternion test1 = quat1 + quat2;
Quaternion test11(-6, 52, 2, 8);
test11 += quat1;
test(test1 == Quaternion(2, 12, 10, 13)); test(test1 == Quaternion(2, 12, 10, 13));
test(test11 == Quaternion(-2, 57, 4, 18));
// Test substraction // Test substraction
Quaternion test2 = quat1 - quat2; Quaternion test2 = quat1 - quat2;
Quaternion test22(-73, 62, 25, 9);
test22 -= quat1;
test(test2 == Quaternion(6, -2, -6, 7)); test(test2 == Quaternion(6, -2, -6, 7));
test(test22 == Quaternion(-77, 57, 23, -1));
// Test multiplication with a number // Test multiplication with a number
Quaternion test3 = quat1 * 3.0; Quaternion test3 = quat1 * 3.0;