/** * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space * coordinates */ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); mIsInertiaTensorSetByUser = true; if (mType != BodyType::DYNAMIC) return; // Compute the inverse local inertia tensor mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string()); }
// Recompute the center of mass, total mass and inertia tensor of the body using all // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { mInitMass = decimal(0.0); mMassInverse = decimal(0.0); if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero(); if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero(); if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero(); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); // If it is a STATIC or a KINEMATIC body if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { mCenterOfMassWorld = mTransform.getPosition(); return; } assert(mType == BodyType::DYNAMIC); // Compute the total mass of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { mInitMass += shape->getMass(); if (!mIsCenterOfMassSetByUser) { mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass(); } } if (mInitMass > decimal(0.0)) { mMassInverse = decimal(1.0) / mInitMass; } else { mCenterOfMassWorld = mTransform.getPosition(); return; } // Compute the center of mass const Vector3 oldCenterOfMass = mCenterOfMassWorld; if (!mIsCenterOfMassSetByUser) { mCenterOfMassLocal *= mMassInverse; } mCenterOfMassWorld = mTransform * mCenterOfMassLocal; if (!mIsInertiaTensorSetByUser) { // Compute the inertia tensor using all the collision shapes for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) { // Get the inertia tensor of the collision shape in its local-space Matrix3x3 inertiaTensor; shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); // Convert the collision shape inertia tensor into the local-space of the body const Transform& shapeTransform = shape->getLocalToBodyTransform(); Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // center into a inertia tensor w.r.t to the body origin. Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; decimal offsetSquare = offset.lengthSquare(); Matrix3x3 offsetMatrix; offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0)); offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare); offsetMatrix[0] += offset * (-offset.x); offsetMatrix[1] += offset * (-offset.y); offsetMatrix[2] += offset * (-offset.z); offsetMatrix *= shape->getMass(); inertiaTensorLocal += inertiaTensor + offsetMatrix; } // Compute the local inverse inertia tensor mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse(); } // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); }
// Initialize before solving the constraint void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { // Initialize the bodies index in the velocity array mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; // Get the bodies positions and orientations const Vector3& x1 = mBody1->mCenterOfMassWorld; const Vector3& x2 = mBody2->mCenterOfMassWorld; const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); // Get the inertia tensor of bodies mI1 = mBody1->getInertiaTensorInverseWorld(); mI2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; mR2World = orientationBody2 * mLocalAnchorPointBody2; // Compute the current angle around the hinge axis decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); // Check if the limit constraints are violated or not decimal lowerLimitError = hingeAngle - mLowerLimit; decimal upperLimitError = mUpperLimit - hingeAngle; bool oldIsLowerLimitViolated = mIsLowerLimitViolated; mIsLowerLimitViolated = lowerLimitError <= 0; if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { mImpulseLowerLimit = 0.0; } bool oldIsUpperLimitViolated = mIsUpperLimitViolated; mIsUpperLimitViolated = upperLimitError <= 0; if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { mImpulseUpperLimit = 0.0; } // Compute vectors needed in the Jacobian mA1 = orientationBody1 * mHingeLocalAxisBody1; Vector3 a2 = orientationBody2 * 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); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrixTranslation.setToZero(); if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute the bias "b" of the translation constraints mBTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); } // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 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->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { mInverseMassMatrixRotation = matrixKRotation.getInverse(); } // Compute the bias "b" of the rotation constraints mBRotation.setToZero(); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { // Reset all the accumulated impulses mImpulseTranslation.setToZero(); mImpulseRotation.setToZero(); mImpulseLowerLimit = 0.0; mImpulseUpperLimit = 0.0; mImpulseMotor = 0.0; } // If the motor or limits are enabled if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); if (mIsLimitEnabled) { // Compute the bias "b" of the lower limit constraint mBLowerLimit = 0.0; if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBLowerLimit = biasFactor * lowerLimitError; } // Compute the bias "b" of the upper limit constraint mBUpperLimit = 0.0; if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { mBUpperLimit = biasFactor * upperLimitError; } } } }
// 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->mMassInverse; decimal inverseMassBody2 = mBody2->mMassInverse; // 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 = mBody1->mMassInverse + mBody2->mMassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrixTranslation.setToZero(); if (mBody1->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { 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); // Compute the impulse of body 1 Vector3 linearImpulseBody1 = -lambdaTranslation; Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3 w1 = mI1 * 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(mR2World); // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * lambdaTranslation; Vector3 w2 = mI2 * 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 mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; Vector3 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->getType() == DYNAMIC || mBody2->getType() == DYNAMIC) { 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); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 1 w1 = mI1 * 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 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 2 w2 = mI2 * angularImpulseBody2; // Update the body position/orientation of body 2 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 = mA1.dot(mI1 * mA1) + 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 ); // Compute the impulse P=J^T * lambda of body 1 const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 1 const Vector3 w1 = mI1 * 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 of body 2 const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 2 const Vector3 w2 = mI2 * angularImpulseBody2; // Update the body position/orientation of body 2 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); // Compute the impulse P=J^T * lambda of body 1 const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 1 const Vector3 w1 = mI1 * 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 of body 2 const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 2 const Vector3 w2 = mI2 * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); } } }