diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d2218136..d8c3a644 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,14 +36,13 @@ 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, - SingleFrameAllocator& singleFrameAllocator) + SingleFrameAllocator& allocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mSingleFrameAllocator(singleFrameAllocator), - mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr), + mContactConstraints(nullptr), mSingleFrameAllocator(allocator), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), @@ -51,59 +50,33 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity } -// Initialize the contact constraints -void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { - - mTimeStep = timeStep; - - // TODO : Try not to count manifolds here - // Count the contact manifolds - uint nbContactManifolds = 0; - for (uint i = 0; i < nbIslands; i++) { - nbContactManifolds += islands[i]->getNbContactManifolds(); - } - - mNbFrictionConstraints = 0; - mNbPenetrationConstraints = 0; - - mPenetrationConstraints = nullptr; - mFrictionConstraints = nullptr; - - if (nbContactManifolds == 0) return; - - // TODO : Count exactly the number of constraints to allocate here - uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD; - mPenetrationConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints)); - assert(mPenetrationConstraints != nullptr); - - mFrictionConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); - assert(mFrictionConstraints != nullptr); - - // For each island of the world - for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { - initializeForIsland(islands[islandIndex]); - } - - // Warmstarting - warmStart(); -} - // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(Island* island) { +void ContactSolver::initializeForIsland(decimal dt, Island* island) { PROFILE("ContactSolver::initializeForIsland()"); assert(island != nullptr); assert(island->getNbBodies() > 0); + assert(island->getNbContactManifolds() > 0); 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++) { + for (uint i=0; igetNbContactPoints() > 0); // Get the two bodies of the contact @@ -112,535 +85,692 @@ void ContactSolver::initializeForIsland(Island* island) { assert(body1 != nullptr); assert(body2 != nullptr); - // TODO : Check if we have a better way to find the body index - uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - - new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint(); - mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1; - mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2; - mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold; - // Get the position of the two bodies const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x2 = body2->mCenterOfMassWorld; - // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[indexBody1]; - const Vector3& w1 = mAngularVelocities[indexBody1]; - const Vector3& v2 = mLinearVelocities[indexBody2]; - const Vector3& w2 = mAngularVelocities[indexBody2]; - - // Get the inertia tensors of both bodies - Matrix3x3 I1 = body1->getInertiaTensorInverseWorld(); - Matrix3x3 I2 = body2->getInertiaTensorInverseWorld(); - - mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 = I1; - mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 = I2; - // Initialize the internal contact manifold structure using the external // contact manifold - mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse; - mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse; - decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); - mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; + 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; - bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; - - Vector3 frictionPointBody1; - Vector3 frictionPointBody2; - mFrictionConstraints[mNbFrictionConstraints].normal.setToZero(); - - // Compute the inverse K matrix for the rolling resistance constraint - mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero(); - if (mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { - mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = I1 + I2; - mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.getInverse(); + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1 = Vector3::zero(); + internalManifold.frictionPointBody2 = Vector3::zero(); } - int nbContacts = 0; - // 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); - new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint(); - mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1; - mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2; - mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1; - mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2; - mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse; - mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse; - mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; - mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact; - // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1; - mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2; - mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal(); - mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth(); - mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact(); - - mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact; - + contactPoint.externalContact = externalContact; + contactPoint.normal = externalContact->getNormal(); + contactPoint.r1 = p1 - x1; + contactPoint.r2 = p2 - x2; + contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); + contactPoint.isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0; + contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); + contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + contactPoint.rollingResistanceImpulse = Vector3::zero(); - frictionPointBody1 += p1; - frictionPointBody2 += p2; + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1 += p1; + internalManifold.frictionPointBody2 += p2; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + 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(); + + // If warm starting is active + if (mIsWarmStartingActive) { + + // Initialize the accumulated impulses with the previous step accumulated impulses + internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + } + else { + + // Initialize the accumulated impulses to zero + internalManifold.friction1Impulse = 0.0; + internalManifold.friction2Impulse = 0.0; + internalManifold.frictionTwistImpulse = 0.0; + internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0); + } + } + } + + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); +} + +// Initialize the contact constraints before solving the system +void ContactSolver::initializeContactConstraints() { + + // For each contact constraint + for (uint c=0; c decimal(0.0) ? mPenetrationConstraints[mNbPenetrationConstraints].inversePenetrationMass = decimal(1.0) / + decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + + ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); + massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0); + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // Compute the friction vectors + computeFrictionVectors(deltaV, contactPoint); + + contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); + contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); + contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); + contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); + + // Compute the inverse mass matrix K for the friction + // constraints at each contact point + decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( + contactPoint.frictionVector1) + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( + contactPoint.frictionVector1); + decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( + contactPoint.frictionVector2) + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( + contactPoint.frictionVector2); + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / + friction1Mass : + decimal(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / + friction2Mass : + 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 - mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); + contactPoint.restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(contactPoint.normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = restitutionFactor * deltaVDotN; + contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } // If the warm starting of the contact solver is active if (mIsWarmStartingActive) { // Get the cached accumulated impulses from the previous step - mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = externalContact->getPenetrationImpulse(); + contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); + contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); + contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); + contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); } // Initialize the split impulses to zero - mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0; + contactPoint.penetrationSplitImpulse = 0.0; - mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal; - - mNbPenetrationConstraints++; - nbContacts++; + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + manifold.normal += contactPoint.normal; + } } - frictionPointBody1 /= nbContacts; - frictionPointBody2 /= nbContacts; - mFrictionConstraints[mNbFrictionConstraints].r1Friction = frictionPointBody1 - x1; - mFrictionConstraints[mNbFrictionConstraints].r2Friction = frictionPointBody2 - x2; - mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1(); - mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2(); - - // If warm starting is active - if (mIsWarmStartingActive) { - - // Initialize the accumulated impulses with the previous step accumulated impulses - mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1(); - mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2(); - mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); - } - else { - - // Initialize the accumulated impulses to zero - mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0; - mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0; - mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0; - mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0); + // Compute the inverse K matrix for the rolling resistance constraint + manifold.inverseRollingResistance.setToZero(); + if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { + manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; + manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); } - mFrictionConstraints[mNbFrictionConstraints].normal.normalize(); + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { - Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - - v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); + manifold.normal.normalize(); - // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]); + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); - // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold - mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); - mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); - mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1); - mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2); - decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + - ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( - mFrictionConstraints[mNbFrictionConstraints].frictionVector1) + - ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( - mFrictionConstraints[mNbFrictionConstraints].frictionVector1); - decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 + - ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot( - mFrictionConstraints[mNbFrictionConstraints].frictionVector2) + - ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot( - mFrictionConstraints[mNbFrictionConstraints].frictionVector2); - decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 * - mFrictionConstraints[mNbFrictionConstraints].normal) + - mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 * - mFrictionConstraints[mNbFrictionConstraints].normal); - friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass - : decimal(0.0); - friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass - : decimal(0.0); - frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) / - frictionTwistMass : - decimal(0.0); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, manifold); - mNbFrictionConstraints++; + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); + decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( + manifold.frictionVector1) + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( + manifold.frictionVector1); + decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( + manifold.frictionVector2) + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( + manifold.frictionVector2); + decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * + manifold.normal) + + manifold.normal.dot(manifold.inverseInertiaTensorBody2 * + manifold.normal); + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass + : decimal(0.0); + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass + : decimal(0.0); + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / + frictionTwistMass : + decimal(0.0); + } } } -// Solve the contact constraints of one iteration of the solve -void ContactSolver::solve() { - - assert(mTimeStep > decimal(0.0)); - - resetTotalPenetrationImpulse(); - solvePenetrationConstraints(); - solveFrictionConstraints(); -} - // Warm start the solver. /// For each constraint, we apply the previous impulse (from the previous step) /// at the beginning. With this technique, we will converge faster towards /// the solution of the linear system void ContactSolver::warmStart() { - PROFILE("ContactSolver::warmStart()"); + // Check that warm starting is active + if (!mIsWarmStartingActive) return; - // Penetration constraints - for (uint i=0; i 0) { + + // Compute the impulse P = J^T * lambda + const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, + Vector3::zero(), contactPoint.rollingResistanceImpulse); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRollingResistance, contactManifold); + } + } + } + else { // If it is a new contact point + + // Initialize the accumulated impulses to zero + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + contactPoint.rollingResistanceImpulse = Vector3::zero(); + } } - else { // If it is a new contact point - - // Initialize the accumulated impulses to zero - mPenetrationConstraints[i].penetrationImpulse = 0.0; - } - } - - // Friction constraints - for (uint i=0; i SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(mPenetrationConstraints[i].penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + mPenetrationConstraints[i].restitutionBias; + ContactPointSolver& contactPoint = contactManifold.contacts[i]; - // Compute the Lagrange multiplier lambda - if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + mPenetrationConstraints[i].restitutionBias) * - mPenetrationConstraints[i].inversePenetrationMass; - } - else { - deltaLambda = - (Jv + b) * mPenetrationConstraints[i].inversePenetrationMass; - } - lambdaTemp = mPenetrationConstraints[i].penetrationImpulse; - mPenetrationConstraints[i].penetrationImpulse = std::max(mPenetrationConstraints[i].penetrationImpulse + - deltaLambda, decimal(0.0)); - deltaLambda = mPenetrationConstraints[i].penetrationImpulse - lambdaTemp; - - // Add the penetration impulse to the total impulse of the corresponding friction constraint - mFrictionConstraints[mPenetrationConstraints[i].indexFrictionConstraint].totalPenetrationImpulse += mPenetrationConstraints[i].penetrationImpulse; - - // Update the velocities of the body 1 by applying the impulse P=J^T * lambda - Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambda; - v1 += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse); - w1 += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambda); - - // Update the velocities of the body 1 by applying the impulse P=J^T * lambda - v2 += mPenetrationConstraints[i].massInverseBody2 * linearImpulse; - w2 += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambda); - - // If the split impulse position correction is active - if (mIsSplitImpulseActive) { - - // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(mPenetrationConstraints[i].r2) - - v1Split - w1Split.cross(mPenetrationConstraints[i].r1); - decimal JvSplit = deltaVSplit.dot(mPenetrationConstraints[i].normal); - decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - mPenetrationConstraints[i].inversePenetrationMass; - decimal lambdaTempSplit = mPenetrationConstraints[i].penetrationSplitImpulse; - mPenetrationConstraints[i].penetrationSplitImpulse = std::max( - mPenetrationConstraints[i].penetrationSplitImpulse + - deltaLambdaSplit, decimal(0.0)); - deltaLambdaSplit = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit; - - // Update the velocities of the body 1 by applying the impulse P=J^T * lambda - Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambdaSplit; - mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse); - mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambdaSplit); - - // Update the velocities of the body 1 by applying the impulse P=J^T * lambda - mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 * linearImpulse; - mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambdaSplit); - } - } -} - -// Solve the friction constraints -void ContactSolver::solveFrictionConstraints() { - - PROFILE("ContactSolver::solveFrictionConstraints()"); - - for (uint i=0; i 0) { + // --------- Penetration --------- // // Compute J*v - const Vector3 JvRolling = w2 - w1; + Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + decimal deltaVDotN = deltaV.dot(contactPoint.normal); + decimal Jv = deltaVDotN; + + // Compute the bias "b" of the constraint + decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; + decimal biasPenetrationDepth = 0.0; + if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + contactPoint.restitutionBias; // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = mFrictionConstraints[i].inverseRollingResistance * (-JvRolling); - decimal rollingLimit = mFrictionConstraints[i].rollingResistanceFactor * mFrictionConstraints[i].totalPenetrationImpulse; - Vector3 lambdaTempRolling = mFrictionConstraints[i].rollingResistanceImpulse; - mFrictionConstraints[i].rollingResistanceImpulse = clamp(mFrictionConstraints[i].rollingResistanceImpulse + - deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = mFrictionConstraints[i].rollingResistanceImpulse - lambdaTempRolling; + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contactPoint.restitutionBias) * + contactPoint.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + } + lambdaTemp = contactPoint.penetrationImpulse; + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + deltaLambda, decimal(0.0)); + deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -deltaLambdaRolling; - angularImpulseBody2 = deltaLambdaRolling; + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, + contactPoint); - // Update the velocities of the body 1 by applying the impulse P - w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1; + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, contactManifold); - // Update the velocities of the body 1 by applying the impulse P - w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2; + sumPenetrationImpulse += contactPoint.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); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * + contactPoint.inversePenetrationMass; + decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = std::max( + contactPoint.penetrationSplitImpulse + + deltaLambdaSplit, decimal(0.0)); + deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + + // Compute the impulse P=J^T * lambda + const Impulse splitImpulsePenetration = computePenetrationImpulse( + deltaLambdaSplit, contactPoint); + + applySplitImpulse(splitImpulsePenetration, contactManifold); + } + + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // --------- Friction 1 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector1); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction1Impulse; + contactPoint.friction1Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction1Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, + contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // --------- Friction 2 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector2); + + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * + contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction2Impulse; + contactPoint.friction2Impulse = std::max(-frictionLimit, + std::min(contactPoint.friction2Impulse + + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, + contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + + // --------- Rolling resistance constraint --------- // + + if (contactManifold.rollingResistanceFactor > 0) { + + // Compute J*v + const Vector3 JvRolling = w2 - w1; + + // Compute the Lagrange multiplier lambda + Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; + Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; + contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + + deltaLambdaRolling, rollingLimit); + deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; + + // Compute the impulse P=J^T * lambda + const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, + Vector3::zero(), deltaLambdaRolling); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRolling, contactManifold); + } + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + // ------ 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); + + // 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 + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // ------ 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); + + // 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, frictionLimit)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(contactManifold.normal); + + deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.frictionTwistImpulse; + contactManifold.frictionTwistImpulse = std::max(-frictionLimit, + std::min(contactManifold.frictionTwistImpulse + + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = contactManifold.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); + + // --------- Rolling resistance constraint at the center of the contact manifold --------- // + + if (contactManifold.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 + + deltaLambdaRolling, rollingLimit); + deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; + + // Compute the impulse P=J^T * lambda + angularImpulseBody1 = -deltaLambdaRolling; + angularImpulseBody2 = deltaLambdaRolling; + const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, + Vector3::zero(), angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseRolling, contactManifold); + } } } } @@ -649,32 +779,76 @@ void ContactSolver::solveFrictionConstraints() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { - // Penetration constraints - for (uint i=0; isetPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse); - } + // For each contact manifold + for (uint c=0; csetFrictionImpulse1(mFrictionConstraints[i].friction1Impulse); - mFrictionConstraints[i].contactManifold->setFrictionImpulse2(mFrictionConstraints[i].friction2Impulse); - mFrictionConstraints[i].contactManifold->setFrictionTwistImpulse(mFrictionConstraints[i].frictionTwistImpulse); - mFrictionConstraints[i].contactManifold->setRollingResistanceImpulse(mFrictionConstraints[i].rollingResistanceImpulse); - mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1); - mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2); + for (uint i=0; isetPenetrationImpulse(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); + } + + 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); } } -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, - FrictionConstraint& frictionConstraint) const { +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applyImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { - assert(frictionConstraint.normal.length() > MACHINE_EPSILON); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; +} + +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { + + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver& contactPoint) const { + + assert(contactPoint.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(frictionConstraint.normal) * frictionConstraint.normal; + Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; Vector3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero @@ -683,15 +857,54 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Compute the first friction vector in the direction of the tangent // velocity difference - frictionConstraint.frictionVector1 = tangentVelocity / lengthTangenVelocity; + contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; } else { // Get any orthogonal vector to the normal as the first friction vector - frictionConstraint.frictionVector1 = frictionConstraint.normal.getOneUnitOrthogonalVector(); + contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); } // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit(); + contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contact) const { + + assert(contact.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > MACHINE_EPSILON) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // 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 0e710c6f..7fd4442f 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -31,7 +31,6 @@ #include "configuration.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" -#include "memory/SingleFrameAllocator.h" #include "Island.h" #include "Impulse.h" #include @@ -40,6 +39,7 @@ /// ReactPhysics3D namespace namespace reactphysics3d { + // Class Contact Solver /** * This class represents the contact solver that is used to solve rigid bodies contacts. @@ -113,100 +113,30 @@ class ContactSolver { private: - struct PenetrationConstraint { - - /// Index of body 1 in the constraint solver - uint indexBody1; - - /// Index of body 2 in the constraint solver - uint indexBody2; - - /// Normal vector of the contact - Vector3 normal; - - /// Vector from the body 1 center to the contact point - Vector3 r1; - - /// Vector from the body 2 center to the contact point - Vector3 r2; - - /// Cross product of r1 with the contact normal - Vector3 r1CrossN; - - /// Cross product of r2 with the contact normal - Vector3 r2CrossN; - - /// Penetration depth - decimal penetrationDepth; - - /// Velocity restitution bias - decimal restitutionBias; + // Structure ContactPointSolver + /** + * Contact solver internal data structure that to store all the + * information relative to a contact point + */ + struct ContactPointSolver { /// Accumulated normal impulse decimal penetrationImpulse; - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; - - /// Inverse of the mass of body 1 - decimal massInverseBody1; - - /// Inverse of the mass of body 2 - decimal massInverseBody2; - - /// Inverse of the matrix K for the penenetration - decimal inversePenetrationMass; - - /// Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody1; - - /// Inverse inertia tensor of body 2 - Matrix3x3 inverseInertiaTensorBody2; - - /// Index of the corresponding friction constraint - uint indexFrictionConstraint; - - /// Pointer to the corresponding contact point - ContactPoint* contactPoint; - - /// True if this constraint is for a resting contact - bool isRestingContact; - }; - - struct FrictionConstraint { - - /// Index of body 1 in the constraint solver - uint indexBody1; - - /// Index of body 2 in the constraint solver - uint indexBody2; - - /// R1 vector for the friction constraints - Vector3 r1Friction; - - /// R2 vector for the friction constraints - Vector3 r2Friction; - - /// Average normal vector of the contact manifold - Vector3 normal; - /// Accumulated impulse in the 1st friction direction decimal friction1Impulse; /// Accumulated impulse in the 2nd friction direction decimal friction2Impulse; - /// Twist friction impulse at contact manifold center - decimal frictionTwistImpulse; + /// Accumulated split impulse for penetration correction + decimal penetrationSplitImpulse; /// Accumulated rolling resistance impulse Vector3 rollingResistanceImpulse; - /// Rolling resistance factor between the two bodies - decimal rollingResistanceFactor; - - /// Mix friction coefficient for the two bodies - decimal frictionCoefficient; + /// Normal vector of the contact + Vector3 normal; /// First friction vector in the tangent plane Vector3 frictionVector1; @@ -214,12 +144,18 @@ class ContactSolver { /// Second friction vector in the tangent plane Vector3 frictionVector2; - /// Old 1st friction direction at contact manifold center + /// Old first friction vector in the tangent plane Vector3 oldFrictionVector1; - /// Old 2nd friction direction at contact manifold center + /// Old second friction vector in the tangent plane Vector3 oldFrictionVector2; + /// Vector from the body 1 center to the contact point + Vector3 r1; + + /// Vector from the body 2 center to the contact point + Vector3 r2; + /// Cross product of r1 with 1st friction vector Vector3 r1CrossT1; @@ -232,8 +168,20 @@ class ContactSolver { /// Cross product of r2 with 2nd friction vector Vector3 r2CrossT2; - /// Total of the all the corresponding penetration impulses - decimal totalPenetrationImpulse; + /// Cross product of r1 with the contact normal + Vector3 r1CrossN; + + /// Cross product of r2 with the contact normal + Vector3 r2CrossN; + + /// Penetration depth + decimal penetrationDepth; + + /// Velocity restitution bias + decimal restitutionBias; + + /// Inverse of the matrix K for the penenetration + decimal inversePenetrationMass; /// Inverse of the matrix K for the 1st friction decimal inverseFriction1Mass; @@ -241,16 +189,30 @@ class ContactSolver { /// Inverse of the matrix K for the 2nd friction decimal inverseFriction2Mass; - /// Matrix K for the twist friction constraint - decimal inverseTwistFrictionMass; + /// True if the contact was existing last time step + bool isRestingContact; - /// Matrix K for the rolling resistance constraint - Matrix3x3 inverseRollingResistance; + /// Pointer to the external contact + ContactPoint* externalContact; + }; + + // Structure ContactManifoldSolver + /** + * Contact solver internal data structure to store all the + * information relative to a contact manifold. + */ + struct ContactManifoldSolver { + + /// Index of body 1 in the constraint solver + uint indexBody1; + + /// Index of body 2 in the constraint solver + uint indexBody2; /// Inverse of the mass of body 1 decimal massInverseBody1; - /// Inverse of the mass of body 2 + // Inverse of the mass of body 2 decimal massInverseBody2; /// Inverse inertia tensor of body 1 @@ -259,11 +221,94 @@ class ContactSolver { /// Inverse inertia tensor of body 2 Matrix3x3 inverseInertiaTensorBody2; - /// Pointer to the corresponding contact manifold - ContactManifold* contactManifold; + /// Contact point constraints + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - /// True if the original contact manifold has at least one resting contact - bool hasAtLeastOneRestingContactPoint; + /// Number of contact points + uint nbContacts; + + /// True if the body 1 is of type dynamic + bool isBody1DynamicType; + + /// True if the body 2 is of type dynamic + bool isBody2DynamicType; + + /// Mix of the restitution factor for two bodies + decimal restitutionFactor; + + /// Mix friction coefficient for the two bodies + decimal frictionCoefficient; + + /// Rolling resistance factor between the two bodies + decimal rollingResistanceFactor; + + /// Pointer to the external contact manifold + ContactManifold* externalContactManifold; + + // - Variables used when friction constraints are apply at the center of the manifold-// + + /// Average normal vector of the contact manifold + Vector3 normal; + + /// Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody1; + + /// Point on body 2 where to apply the friction constraints + Vector3 frictionPointBody2; + + /// R1 vector for the friction constraints + Vector3 r1Friction; + + /// R2 vector for the friction constraints + Vector3 r2Friction; + + /// Cross product of r1 with 1st friction vector + Vector3 r1CrossT1; + + /// Cross product of r1 with 2nd friction vector + Vector3 r1CrossT2; + + /// Cross product of r2 with 1st friction vector + Vector3 r2CrossT1; + + /// Cross product of r2 with 2nd friction vector + Vector3 r2CrossT2; + + /// Matrix K for the first friction constraint + decimal inverseFriction1Mass; + + /// Matrix K for the second friction constraint + decimal inverseFriction2Mass; + + /// Matrix K for the twist friction constraint + decimal inverseTwistFrictionMass; + + /// Matrix K for the rolling resistance constraint + Matrix3x3 inverseRollingResistance; + + /// First friction direction at contact manifold center + Vector3 frictionVector1; + + /// Second friction direction at contact manifold center + Vector3 frictionVector2; + + /// Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector1; + + /// Old 2nd friction direction at contact manifold center + Vector3 oldFrictionVector2; + + /// First friction direction impulse at manifold center + decimal friction1Impulse; + + /// Second friction direction impulse at manifold center + decimal friction2Impulse; + + /// Twist friction impulse at contact manifold center + decimal frictionTwistImpulse; + + /// Rolling resistance impulse + Vector3 rollingResistanceImpulse; }; // -------------------- Constants --------------------- // @@ -285,19 +330,17 @@ class ContactSolver { /// Split angular velocities for the position contact solver (split impulse) Vector3* mSplitAngularVelocities; - /// Reference to the single frame memory allocator - SingleFrameAllocator& mSingleFrameAllocator; - /// Current time step decimal mTimeStep; - PenetrationConstraint* mPenetrationConstraints; + /// Contact constraints + ContactManifoldSolver* mContactConstraints; - FrictionConstraint* mFrictionConstraints; + /// Number of contact constraints + uint mNbContactManifolds; - uint mNbPenetrationConstraints; - - uint mNbFrictionConstraints; + /// Single frame memory allocator + SingleFrameAllocator& mSingleFrameAllocator; /// Array of linear velocities Vector3* mLinearVelocities; @@ -320,15 +363,15 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Initialize the constraint solver for a given island - void initializeForIsland(Island* island); + /// Initialize the contact constraints before solving the system + void initializeContactConstraints(); /// Apply an impulse to the two bodies of a constraint - //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); /// Apply an impulse to the two bodies of a constraint - //void applySplitImpulse(const Impulse& impulse, - // const ContactManifoldSolver& manifold); + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); /// Compute the collision restitution factor from the restitution factor of each body decimal computeMixedRestitutionFactor(RigidBody *body1, @@ -341,30 +384,29 @@ class ContactSolver { /// Compute th mixed rolling resistance factor between two bodies decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; - // TODO : Delete this /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact point. The two vectors have to be /// such that : t1 x t2 = contactNormal. -// void computeFrictionVectors(const Vector3& deltaVelocity, -// ContactPointSolver &contactPoint) const; + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver &contactPoint) const; /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction /// plane for a contact manifold. The two vectors have to be /// such that : t1 x t2 = contactNormal. void computeFrictionVectors(const Vector3& deltaVelocity, - FrictionConstraint& frictionConstraint) const; + ContactManifoldSolver& contactPoint) const; /// Compute a penetration constraint impulse -// const Impulse computePenetrationImpulse(decimal deltaLambda, -// const PenetrationConstraint& constraint) const; + const Impulse computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; /// Compute the first friction constraint impulse const Impulse computeFriction1Impulse(decimal deltaLambda, - const FrictionConstraint& contactPoint) const; + const ContactPointSolver& contactPoint) const; /// Compute the second friction constraint impulse const Impulse computeFriction2Impulse(decimal deltaLambda, - const FrictionConstraint& contactPoint) const; + const ContactPointSolver& contactPoint) const; public: @@ -372,16 +414,13 @@ class ContactSolver { /// Constructor ContactSolver(const std::map& mapBodyToVelocityIndex, - SingleFrameAllocator& singleFrameAllocator); + SingleFrameAllocator& allocator); /// Destructor ~ContactSolver() = default; - /// Initialize the contact constraints - void init(Island** islands, uint nbIslands, decimal timeStep); - - /// Solve the contact constraints of one iteration of the solve - void solve(); + /// Initialize the constraint solver for a given island + void initializeForIsland(decimal dt, Island* island); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -399,16 +438,7 @@ class ContactSolver { void storeImpulses(); /// Solve the contacts - //void solve(); - - /// Reset the total penetration impulse of friction constraints - void resetTotalPenetrationImpulse(); - - /// Solve the penetration constraints - void solvePenetrationConstraints(); - - /// Solve the friction constraints - void solveFrictionConstraints(); + void solve(); /// Return true if the split impulses position correction technique is used for contacts bool isSplitImpulseActive() const; @@ -420,8 +450,8 @@ class ContactSolver { /// the contact manifold instead of solving them at each contact point void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Return true if warmstarting is active - bool IsWarmStartingActive() const; + /// Clean up the constraint solver + void cleanup(); }; // Set the split velocities arrays @@ -476,7 +506,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, RigidBody *body2) const { // Use the geometric mean to compute the mixed friction coefficient - return std::sqrt(body1->getMaterial().getFrictionCoefficient() * + return sqrt(body1->getMaterial().getFrictionCoefficient() * body2->getMaterial().getFrictionCoefficient()); } @@ -487,16 +517,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, } // Compute a penetration constraint impulse -//inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, -// const PenetrationConstraint& constraint) -// const { -// return Impulse(-constraint.normal * deltaLambda, -constraint.r1CrossN * deltaLambda, -// constraint.normal * deltaLambda, constraint.r2CrossN * deltaLambda); -//} +inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, + contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +} // Compute the first friction constraint impulse inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, - const FrictionConstraint& contactPoint) + const ContactPointSolver& contactPoint) const { return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, @@ -506,7 +536,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, // Compute the second friction constraint impulse inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, - const FrictionConstraint& contactPoint) + const ContactPointSolver& contactPoint) const { return Impulse(-contactPoint.frictionVector2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, @@ -514,11 +544,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, contactPoint.r2CrossT2 * deltaLambda); } -// Return true if warmstarting is active -inline bool ContactSolver::IsWarmStartingActive() const { - return mIsWarmStartingActive; -} - } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index b8a48ddc..c40d5ec5 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -331,8 +331,6 @@ void DynamicsWorld::solveContactsAndConstraints() { PROFILE("DynamicsWorld::solveContactsAndConstraints()"); - // TODO : Do not solve per island but solve every constraints at once - // Set the velocities arrays mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, @@ -342,28 +340,25 @@ void DynamicsWorld::solveContactsAndConstraints() { mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, mConstrainedOrientations); - // Initialize the contact solver - mContactSolver.init(mIslands, mNbIslands, mTimeStep); + // ---------- Solve velocity constraints for joints and contacts ---------- // // 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 -// if (mContactSolver.IsWarmStartingActive()) { -// mContactSolver.warmStart(); -// } -// } + // Warm start the contact solver + mContactSolver.warmStart(); + } // If there are constraints if (isConstraintsToSolve) { @@ -371,40 +366,26 @@ 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 constraints + if (isConstraintsToSolve) { + mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); } - mContactSolver.solve(); - // Solve the contacts -// if (isContactsToSolve) { - -// mContactSolver.resetTotalPenetrationImpulse(); - -// mContactSolver.solvePenetrationConstraints(); -// mContactSolver.solveFrictionConstraints(); -// } - } + 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.storeImpulses(); + if (isContactsToSolve) { + mContactSolver.storeImpulses(); + mContactSolver.cleanup(); + } + } } // Solve the position error correction of the constraints