Method initializeForIsland

src/systems/ContactSolverSystem.cpp:115–320

Initialize the constraint solver for a given island

// Initialize the constraint solver for a given island
void ContactSolverSystem::initializeForIsland(uint32 islandIndex) {

    RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);

    assert(mIslands.nbBodiesInIsland[islandIndex] > 0);
    assert(mIslands.nbContactManifolds[islandIndex] > 0);

    // For each contact manifold of the island
    const uint32 contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
    const uint32 nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
    for (uint32 m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {

        ContactManifold& externalManifold = (*mAllContactManifolds)[m];

        assert(externalManifold.nbContactPoints > 0);

        const uint32 rigidBodyIndex1 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity1);
        const uint32 rigidBodyIndex2 = mRigidBodyComponents.getEntityIndex(externalManifold.bodyEntity2);

        const uint32 collider1Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity1);
        const uint32 collider2Index = mColliderComponents.getEntityIndex(externalManifold.colliderEntity2);

        // Get the position of the two bodies
        const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex1];
        const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[rigidBodyIndex2];

        // Initialize the internal contact manifold structure using the external contact manifold
        new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
        mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
        mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex1];
        mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex2];
        mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
        mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
        mContactConstraints[mNbContactManifolds].linearLockAxisFactorBody1 = mRigidBodyComponents.mLinearLockAxisFactors[rigidBodyIndex1];
        mContactConstraints[mNbContactManifolds].linearLockAxisFactorBody2 = mRigidBodyComponents.mLinearLockAxisFactors[rigidBodyIndex2];
        mContactConstraints[mNbContactManifolds].angularLockAxisFactorBody1 = mRigidBodyComponents.mAngularLockAxisFactors[rigidBodyIndex1];
        mContactConstraints[mNbContactManifolds].angularLockAxisFactorBody2 = mRigidBodyComponents.mAngularLockAxisFactors[rigidBodyIndex2];
        mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
        mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]);
        mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
        mContactConstraints[mNbContactManifolds].normal.setToZero();
        mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
        mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();

        // Get the velocities of the bodies
        const Vector3& v1 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex1];
        const Vector3& w1 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex1];
        const Vector3& v2 = mRigidBodyComponents.mLinearVelocities[rigidBodyIndex2];
        const Vector3& w2 = mRigidBodyComponents.mAngularVelocities[rigidBodyIndex2];

        const Transform& collider1LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider1Index];
        const Transform& collider2LocalToWorldTransform = mColliderComponents.mLocalToWorldTransforms[collider2Index];

        // For each  contact point of the contact manifold
        assert(externalManifold.nbContactPoints > 0);
        const uint32 contactPointsStartIndex = externalManifold.contactPointsIndex;
        const uint32 nbContactPoints = static_cast<uint>(externalManifold.nbContactPoints);