From d04cee7d0ae1e8b00da8d700bd11712211f2b17f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Oct 2016 15:40:38 +0200 Subject: [PATCH] Change the way to iterate over contacts --- src/engine/ContactSolver.cpp | 561 ++++++++++++++++++----------------- src/engine/ContactSolver.h | 25 +- src/engine/DynamicsWorld.cpp | 49 +-- 3 files changed, 339 insertions(+), 296 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d02c773d..b4e28aaa 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,7 +36,7 @@ using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP= decimal(0.01); +const decimal ContactSolver::SLOP = decimal(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, @@ -49,8 +49,54 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity } +// Initialize the contact constraints +void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { + + PROFILE("ContactSolver::init()"); + + mTimeStep = timeStep; + + // TODO : Try not to count manifolds and contact points here + uint nbContactManifolds = 0; + uint nbContactPoints = 0; + for (uint i = 0; i < nbIslands; i++) { + uint nbManifoldsInIsland = islands[i]->getNbContactManifolds(); + nbContactManifolds += nbManifoldsInIsland; + + for (uint j=0; j < nbManifoldsInIsland; j++) { + nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints(); + } + } + + mNbContactManifolds = 0; + mNbContactPoints = 0; + + mContactConstraints = nullptr; + mContactPoints = nullptr; + + if (nbContactManifolds == 0 || nbContactPoints == 0) return; + + // TODO : Count exactly the number of constraints to allocate here + mContactPoints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints)); + assert(mContactPoints != nullptr); + + mContactConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds)); + assert(mContactConstraints != nullptr); + + // For each island of the world + for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { + + if (islands[islandIndex]->getNbContactManifolds() > 0) { + initializeForIsland(islands[islandIndex]); + } + } + + // Warmstarting + warmStart(); +} + // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(Island* island) { PROFILE("ContactSolver::initializeForIsland()"); @@ -60,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); - // Set the current time step - mTimeStep = dt; - - mNbContactManifolds = island->getNbContactManifolds(); - - mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; - assert(mContactConstraints != nullptr); - // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifolds(); - for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& internalManifold = mContactConstraints[i]; - assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact @@ -90,34 +126,33 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold - internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.massInverseBody1 = body1->mMassInverse; - internalManifold.massInverseBody2 = body2->mMassInverse; - internalManifold.nbContacts = externalManifold->getNbContactPoints(); - internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; - internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; - internalManifold.normal.setToZero(); - internalManifold.frictionPointBody1 = Vector3::zero(); - internalManifold.frictionPointBody2 = Vector3::zero(); + new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); + mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; + mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse; + mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints(); + mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2); + mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); + mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold; + mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + mContactConstraints[mNbContactManifolds].normal.setToZero(); + mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero(); + mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero(); // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1]; - const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1]; - const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2]; - const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2]; + const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; + const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalManifold.contacts[c]; - // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(c); @@ -125,100 +160,104 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.externalContact = externalContact; - contactPoint.normal = externalContact->getNormal(); - contactPoint.r1 = p1 - x1; - contactPoint.r2 = p2 - x2; - contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); - contactPoint.isRestingContact = externalContact->getIsRestingContact(); + new (mContactPoints + mNbContactPoints) ContactPointSolver(); + mContactPoints[mNbContactPoints].externalContact = externalContact; + mContactPoints[mNbContactPoints].normal = externalContact->getNormal(); + mContactPoints[mNbContactPoints].r1 = p1 - x1; + mContactPoints[mNbContactPoints].r2 = p2 - x2; + mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); + mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); - contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); - contactPoint.penetrationSplitImpulse = 0.0; - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); + mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1(); + mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2(); + mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); + mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; + mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1(); + mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2(); + mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); - internalManifold.frictionPointBody1 += p1; - internalManifold.frictionPointBody2 += p2; + mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; + mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; // Compute the velocity difference - Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); - contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal); - contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); + mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); // Compute the inverse mass matrix K for the penetration constraint - decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + - ((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); - contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); + decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal); + mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); + mContactPoints[mNbContactPoints].restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = internalManifold.restitutionFactor * deltaVDotN; + mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN; } - internalManifold.normal += contactPoint.normal; + mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; + + mNbContactPoints++; } - - internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); - internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); - internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; - internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; - internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); + mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1; + mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2; + mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1(); + mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); // Initialize the accumulated impulses with the previous step accumulated impulses - internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); - internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); - internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1(); + mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2(); + mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); // Compute the inverse K matrix for the rolling resistance constraint - internalManifold.inverseRollingResistance.setToZero(); - if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) { - internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2; - internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse(); + mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); + if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) { + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); } - internalManifold.normal.normalize(); + mContactConstraints[mNbContactManifolds].normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) - - v1 - w1.cross(internalManifold.r1Friction); + Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, internalManifold); + computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold - internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1); - internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2); - internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1); - internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2); - decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot( - internalManifold.frictionVector1) + - ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot( - internalManifold.frictionVector1); - decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot( - internalManifold.frictionVector2) + - ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot( - internalManifold.frictionVector2); - decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 * - internalManifold.normal) + - internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 * - internalManifold.normal); - internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); - internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); - internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); + mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1); + decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2); + decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * + mContactConstraints[mNbContactManifolds].normal) + + mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * + mContactConstraints[mNbContactManifolds].normal); + mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); + + mNbContactManifolds++; } } @@ -228,41 +267,41 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { /// the solution of the linear system void ContactSolver::warmStart() { + uint contactPointIndex = 0; + // For each constraint for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contactPoint.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) * + mContactPoints[contactPointIndex].inversePenetrationMass; } else { - deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass; } - lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse; + mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse + deltaLambda, decimal(0.0)); - deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - Vector3 linearImpulse = contactPoint.normal * deltaLambda; + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda); + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda); // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda); + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda); - sumPenetrationImpulse += contactPoint.penetrationImpulse; + sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; // If the split impulse position correction is active if (mIsSplitImpulseActive) { // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); - decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - + v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); + decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal); decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; - decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = std::max( - contactPoint.penetrationSplitImpulse + + mContactPoints[contactPointIndex].inversePenetrationMass; + decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; + mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max( + mContactPoints[contactPointIndex].penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit; + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); - mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * - (-contactPoint.r1CrossN * deltaLambdaSplit); + mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); + mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * + (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; - mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * - contactPoint.r2CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; + mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * + mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit; } + + contactPointIndex++; } // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) + - v1 - w1.cross(mContactConstraints[c].r1Friction); + decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1); // Compute the Lagrange multiplier lambda - decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction1Impulse + + decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; + decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction1Impulse; + mContactConstraints[c].friction1Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + Jv = deltaV.dot(mContactConstraints[c].frictionVector2); // Compute the Lagrange multiplier lambda - deltaLambda = -Jv * contactManifold.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction2Impulse + + deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction2Impulse; + mContactConstraints[c].friction2Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda; + linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda; + angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; - Jv = deltaV.dot(contactManifold.normal); + Jv = deltaV.dot(mContactConstraints[c].normal); - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = std::max(-frictionLimit, - std::min(contactManifold.frictionTwistImpulse + deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].frictionTwistImpulse; + mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].frictionTwistImpulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody2 = contactManifold.normal * deltaLambda; + angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2); // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // --------- Rolling resistance constraint at the center of the contact manifold --------- // - if (contactManifold.rollingResistanceFactor > 0) { + if (mContactConstraints[c].rollingResistanceFactor > 0) { // Compute J*v const Vector3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; - contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + + Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling); + decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse; + Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse; + mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; + deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling); // Update the velocities of the body 2 by applying the impulse P - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; } } } @@ -548,30 +586,30 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { + uint contactPointIndex = 0; + // For each contact manifold for (uint c=0; csetPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse); + mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse); + mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse); + mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse); - ContactPointSolver& contactPoint = manifold.contacts[i]; + mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1); + mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2); - contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + contactPointIndex++; } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse); + mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse); + mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse); + mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse); + mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1); + mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2); } } @@ -634,12 +672,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } - -// Clean up the constraint solver -void ContactSolver::cleanup() { - - if (mContactConstraints != nullptr) { - delete[] mContactConstraints; - mContactConstraints = nullptr; - } -} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 02a3c390..e4e2710e 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -220,11 +220,8 @@ class ContactSolver { /// Inverse inertia tensor of body 2 Matrix3x3 inverseInertiaTensorBody2; - /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - /// Number of contact points - uint nbContacts; + short int nbContacts; /// True if the body 1 is of type dynamic bool isBody1DynamicType; @@ -335,6 +332,12 @@ class ContactSolver { /// Contact constraints ContactManifoldSolver* mContactConstraints; + /// Contact points + ContactPointSolver* mContactPoints; + + /// Number of contact point constraints + uint mNbContactPoints; + /// Number of contact constraints uint mNbContactManifolds; @@ -378,6 +381,9 @@ class ContactSolver { void computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contactPoint) const; + /// Warm start the solver. + void warmStart(); + public: // -------------------- Methods -------------------- // @@ -389,8 +395,11 @@ class ContactSolver { /// Destructor ~ContactSolver() = default; + /// Initialize the contact constraints + void init(Island** islands, uint nbIslands, decimal timeStep); + /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + void initializeForIsland(Island* island); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -400,9 +409,6 @@ class ContactSolver { void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, Vector3* constrainedAngularVelocities); - /// Warm start the solver. - void warmStart(); - /// Store the computed impulses to use them to /// warm start the solver at the next iteration void storeImpulses(); @@ -415,9 +421,6 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); - - /// Clean up the constraint solver - void cleanup(); }; // Set the split velocities arrays diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index c40d5ec5..d52348f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -342,23 +342,28 @@ void DynamicsWorld::solveContactsAndConstraints() { // ---------- Solve velocity constraints for joints and contacts ---------- // + // Initialize the contact solver + mContactSolver.init(mIslands, mNbIslands, mTimeStep); + // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + //if (!isConstraintsToSolve && !isContactsToSolve) continue; // If there are contacts in the current island - if (isContactsToSolve) { +// if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); +// // Initialize the solver +// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - mContactSolver.warmStart(); - } +// // Warm start the contact solver +// if (mContactSolver.IsWarmStartingActive()) { +// mContactSolver.warmStart(); +// } +// } // If there are constraints if (isConstraintsToSolve) { @@ -366,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() { // Initialize the constraint solver mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); } + } - // For each iteration of the velocity solver - for (uint i=0; igetNbJoints() > 0; if (isConstraintsToSolve) { mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); } - - // Solve the contacts - if (isContactsToSolve) mContactSolver.solve(); } - // Cache the lambda values in order to use them in the next - // step and cleanup the contact solver - if (isContactsToSolve) { - mContactSolver.storeImpulses(); - mContactSolver.cleanup(); - } + mContactSolver.solve(); + + // Solve the contacts +// if (isContactsToSolve) { + +// mContactSolver.resetTotalPenetrationImpulse(); + +// mContactSolver.solvePenetrationConstraints(); +// mContactSolver.solveFrictionConstraints(); +// } } + + mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints