C++ Matrix3x3::getInverse方法代码示例

/**
 * @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
//.........这里部分代码省略.........

// 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;
//.........这里部分代码省略.........