Implement the non-linear-gauss-seidel position error correction
This commit is contained in:
Daniel Chappuis
parent
1c0726d9d6
commit
ce0078c2a9
|
|
@ -239,8 +239,6 @@ inline void CollisionBody::updateOldTransform() {
|
|||
// Update the rigid body in order to reflect a change in the body state
|
||||
inline void CollisionBody::updateAABB() {
|
||||
|
||||
// TODO : An AABB should not be updated every frame but only if the body has moved
|
||||
|
||||
// Update the AABB
|
||||
mCollisionShape->updateAABB(mAabb, mTransform);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -60,6 +60,9 @@ struct BroadPhasePair {
|
|||
/// Destructor
|
||||
~BroadPhasePair();
|
||||
|
||||
/// Return the pair of bodies index
|
||||
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
|
||||
|
||||
/// Return the pair of bodies index
|
||||
bodyindexpair getBodiesIndexPair() const;
|
||||
|
||||
|
|
@ -77,16 +80,22 @@ struct BroadPhasePair {
|
|||
};
|
||||
|
||||
// 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
|
||||
bodyindexpair indexPair = body1->getID() < body2->getID() ?
|
||||
bodyindexpair indexPair = body1->getID() < body2->getID() ?
|
||||
std::make_pair(body1->getID(), body2->getID()) :
|
||||
std::make_pair(body2->getID(), body1->getID());
|
||||
assert(indexPair.first != indexPair.second);
|
||||
return indexPair;
|
||||
}
|
||||
|
||||
// Return the pair of bodies index
|
||||
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
|
||||
|
||||
return computeBodiesIndexPair(body1, body2);
|
||||
}
|
||||
|
||||
// Smaller than operator
|
||||
inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const {
|
||||
return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2));
|
||||
|
|
|
|||
|
|
@ -109,6 +109,9 @@ void CollisionDetection::computeNarrowPhase() {
|
|||
|
||||
// Update the contact cache of the overlapping 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
|
||||
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
|
||||
|
|
|
|||
|
|
@ -79,6 +79,9 @@ class CollisionDetection {
|
|||
/// Narrow-phase Sphere vs Sphere algorithm
|
||||
SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm;
|
||||
|
||||
/// Set of pair of bodies that cannot collide between each other
|
||||
std::set<bodyindexpair> mNoCollisionPairs;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -113,6 +116,12 @@ class CollisionDetection {
|
|||
/// Remove a body from the collision detection
|
||||
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
|
||||
void computeCollisionDetection();
|
||||
|
||||
|
|
@ -148,7 +157,19 @@ inline void CollisionDetection::removeBody(CollisionBody* body) {
|
|||
|
||||
// Remove the body from the broad-phase
|
||||
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));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -143,7 +143,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
|
|||
int minAxis = d.getAbsoluteVector().getMinAxis();
|
||||
|
||||
// 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
|
||||
// v2 and v3
|
||||
|
|
|
|||
|
|
@ -54,15 +54,14 @@ typedef std::pair<bodyindex, bodyindex> bodyindexpair;
|
|||
// ------------------- Enumerations ------------------- //
|
||||
|
||||
/// Position correction technique used in the constraint solver (for joints).
|
||||
/// BAUMGARTE : Faster but can be innacurate in some situations. This is the option
|
||||
/// used by default.
|
||||
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise.
|
||||
/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations.
|
||||
/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default.
|
||||
enum JointsPositionCorrectionTechnique {BAUMGARTE_JOINTS, NON_LINEAR_GAUSS_SEIDEL};
|
||||
|
||||
/// Position correction technique used in the contact solver (for contacts)
|
||||
/// BAUMGARTE : Faster but can be innacurate and can lead to unexpected bounciness
|
||||
/// in some situations (due to error correction factor being added to
|
||||
/// the bodies momentum).
|
||||
/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness
|
||||
/// in some situations (due to error correction factor being added to
|
||||
/// the bodies momentum).
|
||||
/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the
|
||||
/// bodies momentum. This is the option used by default.
|
||||
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
|
||||
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);
|
||||
|
||||
/// 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;
|
||||
|
||||
/// 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;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include "BallAndSocketJoint.h"
|
||||
#include "../engine/ConstraintSolver.h"
|
||||
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
|
|
@ -61,8 +60,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
|||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
mI1 = mBody1->getInertiaTensorInverseWorld();
|
||||
mI2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||
|
|
@ -84,10 +83,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
|||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies);
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
|
||||
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
|
||||
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
|
||||
}
|
||||
|
||||
// Compute the inverse mass matrix K^-1
|
||||
|
|
@ -120,26 +119,29 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
|
|||
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
|
||||
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 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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -152,11 +154,9 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
|
|||
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
|
||||
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 inverseMassBody2 = mBody2->getMassInverse();
|
||||
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Compute J*v
|
||||
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);
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint
|
||||
// Solve the position constraint (for position error correction)
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -81,6 +81,12 @@ class BallAndSocketJoint : public Constraint {
|
|||
/// Vector from center of body 2 to anchor point in world-space
|
||||
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
|
||||
Vector3 mBiasVector;
|
||||
|
||||
|
|
@ -112,7 +118,7 @@ class BallAndSocketJoint : public Constraint {
|
|||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
|
||||
/// Solve the position constraint
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -32,7 +32,8 @@ using namespace reactphysics3d;
|
|||
Constraint::Constraint(const ConstraintInfo& constraintInfo)
|
||||
:mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
|
||||
mType(constraintInfo.type),
|
||||
mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique) {
|
||||
mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique),
|
||||
mIsCollisionEnabled(constraintInfo.isCollisionEnabled){
|
||||
|
||||
assert(mBody1 != NULL);
|
||||
assert(mBody2 != NULL);
|
||||
|
|
|
|||
|
|
@ -59,6 +59,9 @@ struct ConstraintInfo {
|
|||
/// Type of the constraint
|
||||
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).
|
||||
/// By default, the BAUMGARTE technique is used
|
||||
JointsPositionCorrectionTechnique positionCorrectionTechnique;
|
||||
|
|
@ -66,12 +69,14 @@ struct ConstraintInfo {
|
|||
/// Constructor
|
||||
ConstraintInfo(ConstraintType constraintType)
|
||||
: body1(NULL), body2(NULL), type(constraintType),
|
||||
positionCorrectionTechnique(BAUMGARTE_JOINTS) {}
|
||||
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
|
||||
isCollisionEnabled(true) {}
|
||||
|
||||
/// Constructor
|
||||
ConstraintInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, ConstraintType constraintType)
|
||||
: body1(rigidBody1), body2(rigidBody2), type(constraintType),
|
||||
positionCorrectionTechnique(BAUMGARTE_JOINTS) {
|
||||
positionCorrectionTechnique(NON_LINEAR_GAUSS_SEIDEL),
|
||||
isCollisionEnabled(true) {
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
|
|
@ -113,6 +118,9 @@ class Constraint {
|
|||
/// Position correction technique used for the constraint (used for joints)
|
||||
JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
|
||||
|
||||
/// True if the two bodies of the constraint are allowed to collide with each other
|
||||
bool mIsCollisionEnabled;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
|
|
@ -143,6 +151,9 @@ class Constraint {
|
|||
/// Return the type of the constraint
|
||||
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
|
||||
virtual size_t getSizeInBytes() const = 0;
|
||||
|
||||
|
|
@ -177,7 +188,12 @@ inline bool Constraint::isActive() const {
|
|||
// Return the type of the constraint
|
||||
inline ConstraintType Constraint::getType() const {
|
||||
return mType;
|
||||
}
|
||||
}
|
||||
|
||||
// Return true if the collision between the two bodies of the constraint is enabled
|
||||
inline bool Constraint::isCollisionEnabled() const {
|
||||
return mIsCollisionEnabled;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -68,8 +68,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
mI1 = mBody1->getInertiaTensorInverseWorld();
|
||||
mI2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||
|
|
@ -79,7 +79,7 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
|
||||
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;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
inverseMassBodies += mBody1->getMassInverse();
|
||||
|
|
@ -91,10 +91,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies);
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
|
||||
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
|
||||
}
|
||||
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
|
||||
|
|
@ -114,10 +114,10 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
// contraints (3x3 matrix)
|
||||
mInverseMassMatrixRotation.setToZero();
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotation += I1;
|
||||
mInverseMassMatrixRotation += mI1;
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotation += I2;
|
||||
mInverseMassMatrixRotation += mI2;
|
||||
}
|
||||
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
|
||||
|
|
@ -150,32 +150,36 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
|||
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
|
||||
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 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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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
|
||||
|
|
@ -187,11 +191,9 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
|
||||
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 inverseMassBody2 = mBody2->getMassInverse();
|
||||
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
|
|
@ -203,20 +205,25 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
(-JvTranslation - mBiasTranslation);
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
|
@ -228,21 +235,163 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
|
||||
mImpulseRotation += deltaLambda2;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints
|
||||
angularImpulseBody1 = -deltaLambda2;
|
||||
angularImpulseBody2 = deltaLambda2;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
|
||||
// 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
|
||||
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()) {
|
||||
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) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -81,6 +81,12 @@ class FixedJoint : public Constraint {
|
|||
/// Vector from center of body 2 to anchor point in world-space
|
||||
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
|
||||
Vector3 mImpulseTranslation;
|
||||
|
||||
|
|
@ -124,7 +130,7 @@ class FixedJoint : public Constraint {
|
|||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
|
||||
/// Solve the position constraint
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
|
|||
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
|
||||
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
|
||||
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
|
||||
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce) {
|
||||
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) {
|
||||
|
||||
assert(mLowerLimit <= 0 && mLowerLimit >= -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();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
mI1 = mBody1->getInertiaTensorInverseWorld();
|
||||
mI2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||
|
|
@ -107,8 +107,6 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
mImpulseUpperLimit = 0.0;
|
||||
}
|
||||
|
||||
decimal testAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
mA1 = orientationBody1 * mHingeLocalAxisBody1;
|
||||
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
|
||||
|
|
@ -135,10 +133,10 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies);
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
|
||||
massMatrix += skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose();
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
|
||||
massMatrix += skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
|
||||
}
|
||||
mInverseMassMatrixTranslation.setToZero();
|
||||
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
||||
|
|
@ -158,12 +156,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
Vector3 I2B2CrossA1(0, 0, 0);
|
||||
Vector3 I2C2CrossA1(0, 0, 0);
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
I1B2CrossA1 = I1 * mB2CrossA1;
|
||||
I1C2CrossA1 = I1 * mC2CrossA1;
|
||||
I1B2CrossA1 = mI1 * mB2CrossA1;
|
||||
I1C2CrossA1 = mI1 * mC2CrossA1;
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
I2B2CrossA1 = I2 * mB2CrossA1;
|
||||
I2C2CrossA1 = I2 * mC2CrossA1;
|
||||
I2B2CrossA1 = mI2 * mB2CrossA1;
|
||||
I2C2CrossA1 = mI2 * mC2CrossA1;
|
||||
}
|
||||
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
|
||||
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)
|
||||
mInverseMassMatrixLimitMotor = 0.0;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixLimitMotor += mA1.dot(I1 * mA1);
|
||||
mInverseMassMatrixLimitMotor += mA1.dot(mI1 * mA1);
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixLimitMotor += mA1.dot(I2 * mA1);
|
||||
mInverseMassMatrixLimitMotor += mA1.dot(mI2 * mA1);
|
||||
}
|
||||
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 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
|
||||
const decimal inverseMassBody1 = mBody1->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
|
||||
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
|
||||
const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1;
|
||||
angularImpulseBody1 += limitsImpulse;
|
||||
angularImpulseBody2 += -limitsImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint
|
||||
const Vector3 motorImpulse = -mImpulseMotor * mA1;
|
||||
angularImpulseBody1 += motorImpulse;
|
||||
angularImpulseBody2 += -motorImpulse;
|
||||
|
||||
// Apply the impulse to the bodies of the joint
|
||||
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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
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
|
||||
decimal inverseMassBody1 = mBody1->getMassInverse();
|
||||
decimal inverseMassBody2 = mBody2->getMassInverse();
|
||||
Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
|
|
@ -295,20 +306,25 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
(-JvTranslation - mBTranslation);
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
|
@ -321,16 +337,23 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
|
||||
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()) {
|
||||
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()) {
|
||||
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 --------------- //
|
||||
|
|
@ -344,21 +367,26 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
const decimal JvLowerLimit = (w2 - w1).dot(mA1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit - mBLowerLimit);
|
||||
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
|
||||
decimal lambdaTemp = mImpulseLowerLimit;
|
||||
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
|
||||
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()) {
|
||||
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()) {
|
||||
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));
|
||||
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()) {
|
||||
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()) {
|
||||
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);
|
||||
|
||||
// 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 lambdaTemp = mImpulseMotor;
|
||||
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
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()) {
|
||||
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()) {
|
||||
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) {
|
||||
|
||||
// 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
|
||||
void HingeJoint::setMaxMotorForce(decimal maxMotorForce) {
|
||||
// Set the maximum motor torque
|
||||
void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
|
||||
|
||||
if (maxMotorForce != mMaxMotorForce) {
|
||||
if (maxMotorTorque != mMaxMotorTorque) {
|
||||
|
||||
assert(mMaxMotorForce >= 0.0);
|
||||
mMaxMotorForce = maxMotorForce;
|
||||
assert(mMaxMotorTorque >= 0.0);
|
||||
mMaxMotorTorque = maxMotorTorque;
|
||||
|
||||
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||
}
|
||||
|
|
|
|||
|
|
@ -66,8 +66,9 @@ struct HingeJointInfo : public ConstraintInfo {
|
|||
/// Motor speed (in radian/second)
|
||||
decimal motorSpeed;
|
||||
|
||||
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
|
||||
decimal maxMotorForce;
|
||||
/// Maximum motor torque (in Newtons * meters) that can be applied to reach
|
||||
/// to desired motor speed
|
||||
decimal maxMotorTorque;
|
||||
|
||||
/// Constructor without limits and without motor
|
||||
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||
|
|
@ -76,8 +77,8 @@ struct HingeJointInfo : public ConstraintInfo {
|
|||
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(false),
|
||||
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1), motorSpeed(0),
|
||||
maxMotorForce(0) {}
|
||||
isMotorEnabled(false), minAngleLimit(-1), maxAngleLimit(1),
|
||||
motorSpeed(0), maxMotorTorque(0) {}
|
||||
|
||||
/// Constructor with limits but without motor
|
||||
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||
|
|
@ -88,20 +89,21 @@ struct HingeJointInfo : public ConstraintInfo {
|
|||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
|
||||
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
|
||||
maxAngleLimit(initMaxAngleLimit), motorSpeed(0), maxMotorForce(0) {}
|
||||
maxAngleLimit(initMaxAngleLimit), motorSpeed(0),
|
||||
maxMotorTorque(0) {}
|
||||
|
||||
/// Constructor with limits and motor
|
||||
HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||
const Vector3& initAnchorPointWorldSpace,
|
||||
const Vector3& initRotationAxisWorld,
|
||||
decimal initMinAngleLimit, decimal initMaxAngleLimit,
|
||||
decimal initMotorSpeed, decimal initMaxMotorForce)
|
||||
decimal initMotorSpeed, decimal initMaxMotorTorque)
|
||||
: ConstraintInfo(rigidBody1, rigidBody2, HINGEJOINT),
|
||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||
rotationAxisWorld(initRotationAxisWorld), isLimitEnabled(true),
|
||||
isMotorEnabled(false), minAngleLimit(initMinAngleLimit),
|
||||
maxAngleLimit(initMaxAngleLimit), motorSpeed(initMotorSpeed),
|
||||
maxMotorForce(initMaxMotorForce) {}
|
||||
maxMotorTorque(initMaxMotorTorque) {}
|
||||
};
|
||||
|
||||
// Class HingeJoint
|
||||
|
|
@ -132,6 +134,12 @@ class HingeJoint : public Constraint {
|
|||
/// Hinge rotation axis (in local-space coordiantes of body 2)
|
||||
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
|
||||
Vector3 mA1;
|
||||
|
||||
|
|
@ -210,15 +218,16 @@ class HingeJoint : public Constraint {
|
|||
/// Motor speed
|
||||
decimal mMotorSpeed;
|
||||
|
||||
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
|
||||
decimal mMaxMotorForce;
|
||||
/// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
|
||||
decimal mMaxMotorTorque;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Reset the limits
|
||||
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;
|
||||
|
||||
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
|
|
@ -271,14 +280,14 @@ class HingeJoint : public Constraint {
|
|||
/// Set the motor speed
|
||||
void setMotorSpeed(decimal motorSpeed);
|
||||
|
||||
/// Return the maximum motor force
|
||||
decimal getMaxMotorForce() const;
|
||||
/// Return the maximum motor torque
|
||||
decimal getMaxMotorTorque() const;
|
||||
|
||||
/// Set the maximum motor force
|
||||
void setMaxMotorForce(decimal maxMotorForce);
|
||||
/// Set the maximum motor torque
|
||||
void setMaxMotorTorque(decimal maxMotorTorque);
|
||||
|
||||
/// Return the intensity of the current force applied for the joint motor
|
||||
decimal getMotorForce(decimal timeStep) const;
|
||||
/// Return the intensity of the current torque applied for the joint motor
|
||||
decimal getMotorTorque(decimal timeStep) const;
|
||||
|
||||
/// Return the number of bytes used by the joint
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
|
@ -292,7 +301,7 @@ class HingeJoint : public Constraint {
|
|||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
|
||||
/// Solve the position constraint
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
};
|
||||
|
||||
|
|
@ -321,13 +330,13 @@ inline decimal HingeJoint::getMotorSpeed() const {
|
|||
return mMotorSpeed;
|
||||
}
|
||||
|
||||
// Return the maximum motor force
|
||||
inline decimal HingeJoint::getMaxMotorForce() const {
|
||||
return mMaxMotorForce;
|
||||
// Return the maximum motor torque
|
||||
inline decimal HingeJoint::getMaxMotorTorque() const {
|
||||
return mMaxMotorTorque;
|
||||
}
|
||||
|
||||
// Return the intensity of the current force applied for the joint motor
|
||||
inline decimal HingeJoint::getMotorForce(decimal timeStep) const {
|
||||
// Return the intensity of the current torque applied for the joint motor
|
||||
inline decimal HingeJoint::getMotorTorque(decimal timeStep) const {
|
||||
return mImpulseMotor / timeStep;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,9 +36,10 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
|
|||
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
|
||||
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
|
||||
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
|
||||
mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit),
|
||||
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
|
||||
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){
|
||||
mLowerLimit(jointInfo.minTranslationLimit),
|
||||
mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false),
|
||||
mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed),
|
||||
mMaxMotorForce(jointInfo.maxMotorForce){
|
||||
|
||||
assert(mUpperLimit >= 0.0);
|
||||
assert(mLowerLimit <= 0.0);
|
||||
|
|
@ -81,8 +82,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
mI1 = mBody1->getInertiaTensorInverseWorld();
|
||||
mI2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mR1 = orientationBody1 * mLocalAnchorPointBody1;
|
||||
|
|
@ -130,13 +131,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
Vector3 I2R2CrossN2(0, 0, 0);
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
sumInverseMass += mBody1->getMassInverse();
|
||||
I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1;
|
||||
I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2;
|
||||
I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
|
||||
I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
sumInverseMass += mBody2->getMassInverse();
|
||||
I2R2CrossN1 = I2 * mR2CrossN1;
|
||||
I2R2CrossN2 = I2 * mR2CrossN2;
|
||||
I2R2CrossN1 = mI2 * mR2CrossN1;
|
||||
I2R2CrossN2 = mI2 * mR2CrossN2;
|
||||
}
|
||||
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
mR2CrossN1.dot(I2R2CrossN1);
|
||||
|
|
@ -165,10 +166,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
// contraints (3x3 matrix)
|
||||
mInverseMassMatrixRotationConstraint.setToZero();
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotationConstraint += I1;
|
||||
mInverseMassMatrixRotationConstraint += mI1;
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotationConstraint += I2;
|
||||
mInverseMassMatrixRotationConstraint += mI2;
|
||||
}
|
||||
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
|
||||
|
|
@ -189,11 +190,11 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
mInverseMassMatrixLimit = 0.0;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixLimit += mBody1->getMassInverse() +
|
||||
mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis);
|
||||
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis);
|
||||
}
|
||||
if (mBody2->getIsMotionEnabled()) {
|
||||
mInverseMassMatrixLimit += mBody2->getMassInverse() +
|
||||
mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis);
|
||||
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
|
||||
}
|
||||
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 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
|
||||
const decimal inverseMassBody1 = mBody1->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
|
||||
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
|
||||
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
|
||||
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
|
||||
linearImpulseBody1 += impulseMotor;
|
||||
linearImpulseBody2 += -impulseMotor;
|
||||
|
||||
// Apply the impulse to the bodies of the joint
|
||||
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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
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
|
||||
decimal inverseMassBody1 = mBody1->getMassInverse();
|
||||
decimal inverseMassBody2 = mBody2->getMassInverse();
|
||||
const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
|
||||
const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
|
|
@ -313,20 +325,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
|
@ -338,16 +356,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
|
||||
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()) {
|
||||
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()) {
|
||||
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 --------------- //
|
||||
|
|
@ -367,20 +390,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -397,20 +425,25 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
|
||||
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()) {
|
||||
|
||||
// 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;
|
||||
w1 += I1 * angularImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
}
|
||||
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;
|
||||
w2 += I2 * angularImpulseBody2;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -429,23 +462,292 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
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()) {
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor
|
||||
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
|
||||
|
||||
// Apply the impulse to the body
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint
|
||||
// Solve the position constraint (for position error correction)
|
||||
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
|
||||
|
|
@ -469,6 +771,30 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
|
|||
// 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
|
||||
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
|
||||
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ struct SliderJointInfo : public ConstraintInfo {
|
|||
/// Motor speed
|
||||
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;
|
||||
|
||||
/// Constructor without limits and without motor
|
||||
|
|
@ -129,6 +129,12 @@ class SliderJoint : public Constraint {
|
|||
/// Slider axis (in local-space coordinates of body 1)
|
||||
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
|
||||
Quaternion mInitOrientationDifferenceInv;
|
||||
|
||||
|
|
@ -225,7 +231,7 @@ class SliderJoint : public Constraint {
|
|||
/// Motor speed
|
||||
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;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -255,6 +261,9 @@ class SliderJoint : public Constraint {
|
|||
/// Enable/Disable the motor of the joint
|
||||
void enableMotor(bool isMotorEnabled);
|
||||
|
||||
/// Return the current translation value of the joint
|
||||
decimal getTranslation() const;
|
||||
|
||||
/// Return the minimum translation limit
|
||||
decimal getMinTranslationLimit() const;
|
||||
|
||||
|
|
@ -294,7 +303,7 @@ class SliderJoint : public Constraint {
|
|||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
|
||||
/// Solve the position constraint
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -33,14 +33,15 @@ using namespace reactphysics3d;
|
|||
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
|
||||
std::vector<Vector3>& linearVelocities,
|
||||
std::vector<Vector3>& angularVelocities,
|
||||
std::vector<Vector3>& positions,
|
||||
std::vector<Quaternion>& orientations,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||
: mJoints(joints), mLinearVelocities(linearVelocities),
|
||||
mAngularVelocities(angularVelocities),
|
||||
mAngularVelocities(angularVelocities), mPositions(positions),
|
||||
mOrientations(orientations),
|
||||
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||
mIsWarmStartingActive(true),
|
||||
mIsNonLinearGaussSeidelPositionCorrectionActive(false),
|
||||
mConstraintSolverData(linearVelocities,
|
||||
angularVelocities, mapBodyToVelocityIndex){
|
||||
mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
|
||||
angularVelocities, positions, orientations, mapBodyToVelocityIndex){
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -94,10 +95,8 @@ void ConstraintSolver::solveVelocityConstraints() {
|
|||
std::set<Constraint*>::iterator it;
|
||||
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
|
||||
Constraint* joint = (*it);
|
||||
|
||||
// Solve the constraint
|
||||
joint->solveVelocityConstraint(mConstraintSolverData);
|
||||
(*it)->solveVelocityConstraint(mConstraintSolverData);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -110,9 +109,7 @@ void ConstraintSolver::solvePositionConstraints() {
|
|||
std::set<Constraint*>::iterator it;
|
||||
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||
|
||||
Constraint* joint = (*it);
|
||||
|
||||
// Solve the constraint
|
||||
joint->solveVelocityConstraint(mConstraintSolverData);
|
||||
(*it)->solvePositionConstraint(mConstraintSolverData);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,6 +53,12 @@ struct ConstraintSolverData {
|
|||
/// Reference to the bodies angular velocities
|
||||
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
|
||||
/// in the constrained velocities array
|
||||
const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
|
||||
|
|
@ -63,9 +69,12 @@ struct ConstraintSolverData {
|
|||
/// Constructor
|
||||
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
|
||||
std::vector<Vector3>& refAngularVelocities,
|
||||
std::vector<Vector3>& refPositions,
|
||||
std::vector<Quaternion>& refOrientations,
|
||||
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
|
||||
:linearVelocities(refLinearVelocities),
|
||||
angularVelocities(refAngularVelocities),
|
||||
positions(refPositions), orientations(refOrientations),
|
||||
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
|
||||
|
||||
}
|
||||
|
|
@ -161,6 +170,12 @@ class ConstraintSolver {
|
|||
/// after solving the constraints)
|
||||
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
|
||||
/// the constrained velocities array
|
||||
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
||||
|
|
@ -171,9 +186,6 @@ class ConstraintSolver {
|
|||
/// True if the warm starting of the solver is active
|
||||
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
|
||||
ConstraintSolverData mConstraintSolverData;
|
||||
|
||||
|
|
@ -185,6 +197,8 @@ class ConstraintSolver {
|
|||
ConstraintSolver(std::set<Constraint*>& joints,
|
||||
std::vector<Vector3>& linearVelocities,
|
||||
std::vector<Vector3>& angularVelocities,
|
||||
std::vector<Vector3>& positions,
|
||||
std::vector<Quaternion>& orientations,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||
|
||||
/// Destructor
|
||||
|
|
@ -204,16 +218,14 @@ class ConstraintSolver {
|
|||
|
||||
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
|
||||
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
|
||||
inline bool ConstraintSolver::getIsNonLinearGaussSeidelPositionCorrectionActive() const {
|
||||
return mIsNonLinearGaussSeidelPositionCorrectionActive;
|
||||
}
|
||||
|
||||
// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
|
||||
inline void ConstraintSolver::setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive) {
|
||||
mIsNonLinearGaussSeidelPositionCorrectionActive = isActive;
|
||||
// Return true if the body is in at least one constraint
|
||||
inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
||||
return mConstraintBodies.count(body) == 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -442,6 +442,9 @@ class ContactSolver {
|
|||
/// Clean up the constraint solver
|
||||
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
|
||||
void setIsSplitImpulseActive(bool isActive);
|
||||
|
||||
|
|
@ -469,6 +472,11 @@ inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
|
|||
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
|
||||
inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
|
||||
mIsSplitImpulseActive = isActive;
|
||||
|
|
@ -482,7 +490,7 @@ inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool
|
|||
|
||||
// Compute the collision restitution factor from the restitution factor of each body
|
||||
inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1,
|
||||
const RigidBody* body2) const {
|
||||
const RigidBody* body2) const {
|
||||
decimal restitution1 = body1->getRestitution();
|
||||
decimal restitution2 = body2->getRestitution();
|
||||
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
|
|||
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||
mMapBodyToConstrainedVelocityIndex),
|
||||
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||
mConstrainedPositions, mConstrainedOrientations,
|
||||
mMapBodyToConstrainedVelocityIndex),
|
||||
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
||||
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
|
||||
|
|
@ -102,18 +103,24 @@ void DynamicsWorld::update() {
|
|||
// Integrate the velocities
|
||||
integrateRigidBodiesVelocities();
|
||||
|
||||
// Solve the contacts and constraints
|
||||
solveContactsAndConstraints();
|
||||
// Reset the movement boolean variable of each body to false
|
||||
resetBodiesMovementVariable();
|
||||
|
||||
// Update the timer
|
||||
mTimer.nextStep();
|
||||
|
||||
// Reset the movement boolean variable of each body to false
|
||||
resetBodiesMovementVariable();
|
||||
// Solve the contacts and constraints
|
||||
solveContactsAndConstraints();
|
||||
|
||||
// Integrate the position and orientation of each body
|
||||
integrateRigidBodiesPositions();
|
||||
|
||||
// Solve the position correction for constraints
|
||||
solvePositionCorrection();
|
||||
|
||||
// Update the AABBs of the bodies
|
||||
updateRigidBodiesAABB();
|
||||
|
||||
// Cleanup of the contact solver
|
||||
mContactSolver.cleanup();
|
||||
|
||||
|
|
@ -130,7 +137,7 @@ void DynamicsWorld::update() {
|
|||
/// the sympletic Euler time stepping scheme.
|
||||
void DynamicsWorld::integrateRigidBodiesPositions() {
|
||||
|
||||
PROFILE("DynamicsWorld::updateRigidBodiesPositionAndOrientation()");
|
||||
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
|
||||
|
||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
||||
|
||||
|
|
@ -144,9 +151,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
|||
// If the body is allowed to move
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
|
||||
// Update the old Transform of the body
|
||||
rigidBody->updateOldTransform();
|
||||
|
||||
// Get the constrained velocity
|
||||
uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
|
||||
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||
|
|
@ -157,7 +161,9 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
|||
rigidBody->setAngularVelocity(newAngVelocity);
|
||||
|
||||
// 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);
|
||||
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
|
|
@ -168,17 +174,30 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
|||
|
||||
// Compute the new position of the body
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.x,
|
||||
newAngVelocity.y,
|
||||
newAngVelocity.z, 0) *
|
||||
currentOrientation * 0.5 * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
|
||||
currentOrientation * decimal(0.5) * dt;
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
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
|
||||
rigidBody->updateAABB();
|
||||
(*it)->updateAABB();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -210,6 +229,8 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
|||
/// contact solver.
|
||||
void DynamicsWorld::integrateRigidBodiesVelocities() {
|
||||
|
||||
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
|
||||
|
||||
// TODO : Use better memory allocation here
|
||||
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
|
|
@ -231,6 +252,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
|||
dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
|
||||
dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
|
||||
// Update the old Transform of the body
|
||||
rigidBody->updateOldTransform();
|
||||
}
|
||||
|
||||
i++;
|
||||
|
|
@ -274,7 +298,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
|||
// For each iteration of the velocity solver
|
||||
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
|
||||
|
||||
// Solve the velocity constraints
|
||||
// Solve the constraints
|
||||
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
|
||||
|
||||
// Solve the contacts
|
||||
|
|
@ -283,19 +307,60 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
|||
|
||||
// Cache the lambda values in order to use them in the next step
|
||||
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
|
||||
if (mConstraintSolver.getIsNonLinearGaussSeidelPositionCorrectionActive()) {
|
||||
// Do not continue if there is no constraints
|
||||
if (mJoints.empty()) return;
|
||||
|
||||
// For each iteration of the position (error correction) solver
|
||||
for (uint i=0; i<mNbPositionSolverIterations; i++) {
|
||||
// ---------- Get the position/orientation of the rigid bodies ---------- //
|
||||
|
||||
// Solve the position constraints
|
||||
if (isConstraintsToSolve) mConstraintSolver.solvePositionConstraints();
|
||||
// TODO : Use better memory allocation here
|
||||
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;
|
||||
}
|
||||
|
||||
// Destroy a rigid body
|
||||
// Destroy a rigid body and all the joints which it belongs
|
||||
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
||||
|
||||
// Remove the body from the collision detection
|
||||
|
|
@ -377,6 +442,14 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
|||
// Remove the collision shape from the world
|
||||
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
|
||||
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
|
||||
mJoints.insert(newJoint);
|
||||
|
||||
|
|
@ -452,6 +532,13 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
|
|||
|
||||
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
|
||||
mJoints.erase(joint);
|
||||
|
||||
|
|
|
|||
|
|
@ -91,6 +91,12 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// after solving the constraints)
|
||||
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
|
||||
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
|
||||
|
||||
|
|
@ -105,6 +111,9 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Integrate the positions and orientations of rigid bodies.
|
||||
void integrateRigidBodiesPositions();
|
||||
|
||||
/// Update the AABBs of the bodies
|
||||
void updateRigidBodiesAABB();
|
||||
|
||||
/// Update the position and orientation of a body
|
||||
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||
Vector3 newAngVelocity);
|
||||
|
|
@ -118,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
|
|||
/// Solve the contacts and constraints
|
||||
void solveContactsAndConstraints();
|
||||
|
||||
/// Solve the position error correction of the constraints
|
||||
void solvePositionCorrection();
|
||||
|
||||
/// Cleanup the constrained velocities array at each step
|
||||
void cleanupConstrainedVelocitiesArray();
|
||||
|
||||
|
|
@ -137,7 +149,8 @@ class DynamicsWorld : public CollisionWorld {
|
|||
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
|
||||
|
||||
/// 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 :
|
||||
|
||||
|
|
@ -179,7 +192,7 @@ public :
|
|||
const Matrix3x3& inertiaTensorLocal,
|
||||
const CollisionShape& collisionShape);
|
||||
|
||||
/// Destroy a rigid body
|
||||
/// Destroy a rigid body and all the joints which it belongs
|
||||
void destroyRigidBody(RigidBody* rigidBody);
|
||||
|
||||
/// 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
|
||||
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;
|
||||
|
||||
/// 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
|
||||
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator();
|
||||
|
||||
|
|
@ -302,6 +321,11 @@ inline uint DynamicsWorld::getNbRigidBodies() const {
|
|||
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
|
||||
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
|
||||
return mRigidBodies.begin();
|
||||
|
|
@ -317,6 +341,11 @@ inline uint DynamicsWorld::getNbContactManifolds() const {
|
|||
return mContactManifolds.size();
|
||||
}
|
||||
|
||||
/// Return the current physics time (in seconds)
|
||||
inline long double DynamicsWorld::getPhysicsTime() const {
|
||||
return mTimer.getPhysicsTime();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ namespace reactphysics3d {
|
|||
// Class Timer
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
class Timer {
|
||||
|
|
@ -59,9 +59,6 @@ class Timer {
|
|||
/// Timestep dt of the physics engine (timestep > 0.0)
|
||||
double mTimeStep;
|
||||
|
||||
/// Current time of the physics engine
|
||||
long double mTime;
|
||||
|
||||
/// Last time the timer has been updated
|
||||
long double mLastUpdateTime;
|
||||
|
||||
|
|
@ -139,7 +136,7 @@ inline void Timer::setTimeStep(double timeStep) {
|
|||
|
||||
// Return the current time
|
||||
inline long double Timer::getPhysicsTime() const {
|
||||
return mTime;
|
||||
return mLastUpdateTime;
|
||||
}
|
||||
|
||||
// Return if the timer is running
|
||||
|
|
@ -173,9 +170,6 @@ inline bool Timer::isPossibleToTakeStep() const {
|
|||
inline void Timer::nextStep() {
|
||||
assert(mIsRunning);
|
||||
|
||||
// Update the current time of the physics engine
|
||||
mTime += mTimeStep;
|
||||
|
||||
// Update the accumulator value
|
||||
mAccumulator -= mTimeStep;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -133,6 +133,12 @@ struct Quaternion {
|
|||
/// Overloaded operator for the substraction
|
||||
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
|
||||
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);
|
||||
}
|
||||
|
||||
// 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
|
||||
inline Quaternion Quaternion::operator*(decimal nb) const {
|
||||
return Quaternion(nb * x, nb * y, nb * z, nb * w);
|
||||
|
|
|
|||
|
|
@ -197,11 +197,17 @@ class TestQuaternion : public Test {
|
|||
Quaternion quat1(4, 5, 2, 10);
|
||||
Quaternion quat2(-2, 7, 8, 3);
|
||||
Quaternion test1 = quat1 + quat2;
|
||||
Quaternion test11(-6, 52, 2, 8);
|
||||
test11 += quat1;
|
||||
test(test1 == Quaternion(2, 12, 10, 13));
|
||||
test(test11 == Quaternion(-2, 57, 4, 18));
|
||||
|
||||
// Test substraction
|
||||
Quaternion test2 = quat1 - quat2;
|
||||
Quaternion test22(-73, 62, 25, 9);
|
||||
test22 -= quat1;
|
||||
test(test2 == Quaternion(6, -2, -6, 7));
|
||||
test(test22 == Quaternion(-77, 57, 23, -1));
|
||||
|
||||
// Test multiplication with a number
|
||||
Quaternion test3 = quat1 * 3.0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user