Initialize before solving the constraint
| 46 | |
| 47 | // Initialize before solving the constraint |
| 48 | void SolveHingeJointSystem::initBeforeSolve() { |
| 49 | |
| 50 | const decimal biasFactor = (BETA / mTimeStep); |
| 51 | |
| 52 | // For each joint |
| 53 | const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents(); |
| 54 | for (uint32 i=0; i < nbJoints; i++) { |
| 55 | |
| 56 | const Entity jointEntity = mHingeJointComponents.mJointEntities[i]; |
| 57 | const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity); |
| 58 | |
| 59 | // Get the bodies entities |
| 60 | const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; |
| 61 | const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; |
| 62 | |
| 63 | const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); |
| 64 | const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); |
| 65 | |
| 66 | assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity) || !mRigidBodyComponents.getIsEntityDisabled(body2Entity)); |
| 67 | |
| 68 | // Get the inertia tensor of bodies |
| 69 | mHingeJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; |
| 70 | mHingeJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; |
| 71 | |
| 72 | const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); |
| 73 | const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); |
| 74 | |
| 75 | // Compute the vector from body center to the anchor point in world-space |
| 76 | mHingeJointComponents.mR1World[i] = orientationBody1 * (mHingeJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); |
| 77 | mHingeJointComponents.mR2World[i] = orientationBody2 * (mHingeJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); |
| 78 | |
| 79 | // Compute vectors needed in the Jacobian |
| 80 | Vector3& a1 = mHingeJointComponents.mA1[i]; |
| 81 | a1 = orientationBody1 * mHingeJointComponents.mHingeLocalAxisBody1[i]; |
| 82 | Vector3 a2 = orientationBody2 * mHingeJointComponents.mHingeLocalAxisBody2[i]; |
| 83 | |
| 84 | a1.normalize(); |
| 85 | a2.normalize(); |
| 86 | const Vector3 b2 = a2.getOneUnitOrthogonalVector(); |
| 87 | const Vector3 c2 = a2.cross(b2); |
| 88 | mHingeJointComponents.mB2CrossA1[i] = b2.cross(a1); |
| 89 | mHingeJointComponents.mC2CrossA1[i] = c2.cross(a1); |
| 90 | |
| 91 | // Compute the bias "b" of the rotation constraints |
| 92 | mHingeJointComponents.mBiasRotation[i].setToZero(); |
| 93 | if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { |
| 94 | mHingeJointComponents.mBiasRotation[i] = biasFactor * Vector2(a1.dot(b2), a1.dot(c2)); |
| 95 | } |
| 96 | |
| 97 | // Compute the corresponding skew-symmetric matrices |
| 98 | Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); |
| 99 | Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]); |
| 100 | |
| 101 | // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) |
| 102 | decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; |
| 103 | decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; |
| 104 | decimal inverseMassBodies = body1MassInverse + body2MassInverse; |
| 105 | Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, |
no test coverage detected