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

Improve robustness of constraint solver (avoid inverse of matrix with zero determinant)

Browse Source
This commit is contained in:
Daniel Chappuis 2021-01-29 22:08:39 +01:00
parent b18e617c76
commit 2e1046f2b8
8 changed files with 377 additions and 315 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/reactphysics3d/mathematics/Matrix2x2.h
View File

@ -92,6 +92,9 @@ class Matrix2x2 {
/// Return the inverse matrix
Matrix2x2 getInverse() const;
/// Return the inverse matrix
Matrix2x2 getInverse(decimal determinant) const;
/// Return the matrix with absolute values
Matrix2x2 getAbsoluteMatrix() const;
@ -240,6 +243,12 @@ RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::zero() {
return Matrix2x2(0.0, 0.0, 0.0, 0.0);
}
// Return the inverse matrix
RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getInverse() const {
return getInverse(getDeterminant());
}
// Return the matrix with absolute values
RP3D_FORCE_INLINE Matrix2x2 Matrix2x2::getAbsoluteMatrix() const {
return Matrix2x2(std::abs(mRows[0][0]), std::abs(mRows[0][1]),

9
include/reactphysics3d/mathematics/Matrix3x3.h
View File

@ -95,6 +95,9 @@ class Matrix3x3 {
/// Return the inverse matrix
Matrix3x3 getInverse() const;
/// Return the inverse matrix
Matrix3x3 getInverse(decimal determinant) const;
/// Return the matrix with absolute values
Matrix3x3 getAbsoluteMatrix() const;
@ -253,6 +256,12 @@ RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::zero() {
return Matrix3x3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Return the inverse matrix
RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::getInverse() const {
return getInverse(getDeterminant());
}
// Return a skew-symmetric matrix using a given vector that can be used
// to compute cross product with another vector using matrix multiplication
RP3D_FORCE_INLINE Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vector3& vector) {

5
src/mathematics/Matrix2x2.cpp
View File

@ -40,10 +40,7 @@ Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
}
// Return the inverse matrix
Matrix2x2 Matrix2x2::getInverse() const {
// Compute the determinant of the matrix
decimal determinant = getDeterminant();
Matrix2x2 Matrix2x2::getInverse(decimal determinant) const {
// Check if the determinant is equal to zero
assert(std::abs(determinant) > MACHINE_EPSILON);

5
src/mathematics/Matrix3x3.cpp
View File

@ -42,10 +42,7 @@ Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
}
// Return the inverse matrix
Matrix3x3 Matrix3x3::getInverse() const {
// Compute the determinant of the matrix
decimal determinant = getDeterminant();
Matrix3x3 Matrix3x3::getInverse(decimal determinant) const {
// Check if the determinant is equal to zero
assert(determinant != decimal(0.0));

91
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -97,9 +97,12 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
// Compute the inverse mass matrix K^-1
mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse();
decimal massMatrixDeterminant = massMatrix.getDeterminant();
if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant);
}
}
const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
@ -269,45 +272,49 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
skewSymmetricMatrixU1 * mBallAndSocketJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mBallAndSocketJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mBallAndSocketJointComponents.mInverseMassMatrix[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse();
decimal massMatrixDeterminant = massMatrix.getDeterminant();
if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(massMatrixDeterminant);
}
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + r2World - x1 - r1World);
// 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 = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-constraintError);
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1);
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda;
const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute the constraint error (value of the C(x) function)
const Vector3 constraintError = (x2 + r2World - x1 - r1World);
// 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 = mBallAndSocketJointComponents.mInverseMassMatrix[i] * (-constraintError);
// Compute the impulse of body 1
const Vector3 linearImpulseBody1 = -lambda;
const Vector3 angularImpulseBody1 = lambda.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1);
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
const Vector3 angularImpulseBody2 = -lambda.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda;
const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}

200
src/systems/SolveFixedJointSystem.cpp
View File

@ -93,9 +93,12 @@ void SolveFixedJointSystem::initBeforeSolve() {
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
decimal massMatrixDeterminant = massMatrix.getDeterminant();
if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant);
}
}
// Get the bodies positions and orientations
@ -113,9 +116,12 @@ void SolveFixedJointSystem::initBeforeSolve() {
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix)
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i];
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse();
decimal massMatrixRotationDeterminant = mFixedJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
}
}
// Compute the bias "b" for the 3 rotation constraints
@ -336,100 +342,108 @@ void SolveFixedJointSystem::solvePositionConstraint() {
skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
decimal massMatrixDeterminant = massMatrix.getDeterminant();
if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant);
}
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
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)
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i];
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse();
decimal massMatrixRotationDeterminant = mFixedJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
Vector3 angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the pseudo velocity of body 2
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1
w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mFixedJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the pseudo velocity of body 2
w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mFixedJointComponents.mI2[i] * lambdaRotation);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}

171
src/systems/SolveHingeJointSystem.cpp
View File

@ -110,9 +110,12 @@ void SolveHingeJointSystem::initBeforeSolve() {
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrixTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i];
inverseMassMatrixTranslation.setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
decimal massMatrixDeterminant = massMatrix.getDeterminant();
if (std::abs(massMatrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(massMatrixDeterminant);
}
}
// Get the bodies positions and orientations
@ -141,9 +144,12 @@ void SolveHingeJointSystem::initBeforeSolve() {
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
decimal matrixKRotationDeterminant = matrixKRotation.getDeterminant();
if (std::abs(matrixKRotationDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixKRotationDeterminant);
}
}
// If warm-starting is not enabled
@ -500,21 +506,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
}
Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
@ -533,43 +524,63 @@ void SolveHingeJointSystem::solvePositionConstraint() {
c2CrossA1 = c2.cross(a1);
mHingeJointComponents.mC2CrossA1[i] = c2CrossA1;
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
decimal matrixDeterminant = massMatrix.getDeterminant();
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(matrixDeterminant);
}
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// --------------- Rotation Constraints --------------- //
@ -588,37 +599,41 @@ void SolveHingeJointSystem::solvePositionConstraint() {
c2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
matrixDeterminant = matrixKRotation.getDeterminant();
if (std::abs(matrixDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse(matrixDeterminant);
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
Vector3 angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mHingeJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mHingeJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// Compute the current angle around the hinge axis
const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2);

202
src/systems/SolveSliderJointSystem.cpp
View File

@ -185,19 +185,25 @@ void SolveSliderJointSystem::initBeforeSolve() {
r2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant();
if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse();
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant);
}
}
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mSliderJointComponents.mInverseMassMatrixRotation[i] = i1 + i2;
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse();
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
}
}
// Compute the bias "b" of the rotation constraint
@ -609,105 +615,113 @@ void SolveSliderJointSystem::solvePositionConstraint() {
r2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
decimal matrixKTranslationDeterminant = matrixKTranslation.getDeterminant();
if (std::abs(matrixKTranslationDeterminant) > MACHINE_EPSILON) {
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse(matrixKTranslationDeterminant);
}
// Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(n1), u.dot(n2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
r1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(n1), u.dot(n2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
r1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2;
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
// Update the body position/orientation of body 2
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)
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i];
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
decimal massMatrixRotationDeterminant = mSliderJointComponents.mInverseMassMatrixRotation[i].getDeterminant();
if (std::abs(massMatrixRotationDeterminant) > MACHINE_EPSILON) {
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse(massMatrixRotationDeterminant);
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
Vector3 angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body 1
Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
Vector3 angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body 2
Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body 1
w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mSliderJointComponents.mI1[i] * angularImpulseBody1);
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body 2
w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mSliderJointComponents.mI2[i] * angularImpulseBody2);
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Limits Constraints --------------- //
if (mSliderJointComponents.mIsLimitEnabled[i]) {