Method initBeforeSolve

src/systems/SolveFixedJointSystem.cpp:48–142

Initialize before solving the constraint

// Initialize before solving the constraint
void SolveFixedJointSystem::initBeforeSolve() {

    const decimal biasFactor = BETA / mTimeStep;

    // For each joint
    const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
    for (uint32 i=0; i < nbJoints; i++) {

        const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
        const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);

        // Get the bodies entities
        const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
        const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];

        const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
        const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);

        assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity) || !mRigidBodyComponents.getIsEntityDisabled(body2Entity));

        // Get the inertia tensor of bodies
        mFixedJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1];
        mFixedJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2];

        const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
        const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();

        // Compute the vector from body center to the anchor point in world-space
        mFixedJointComponents.mR1World[i] = orientationBody1 * (mFixedJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]);
        mFixedJointComponents.mR2World[i] = orientationBody2 * (mFixedJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]);

        // Compute the corresponding skew-symmetric matrices
        Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
        Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]);

        // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
        const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
        const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
        const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
        Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
                                        0, inverseMassBodies, 0,
                                        0, 0, inverseMassBodies) +
                               skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
                               skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();

        // Compute the inverse mass matrix K^-1 for the 3 translation constraints
        mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
        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
        const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
        const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2];