From e069a25f08bfee44b84ed88ab0b7b65dd7e0b4b7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 10 Sep 2016 11:18:52 +0200 Subject: [PATCH] Start refactoring the contact solver --- src/engine/ContactSolver.cpp | 1308 ++++++++++++++++++++++------------ src/engine/ContactSolver.h | 317 ++++++-- src/engine/DynamicsWorld.cpp | 10 +- src/mathematics/Vector3.cpp | 8 +- 4 files changed, 1114 insertions(+), 529 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 331e8045..22359b10 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -38,12 +38,15 @@ const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP= decimal(0.01); +// TODO : Enable warmstarting again + // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + mContactConstraints(nullptr), mPenetrationConstraints(nullptr), + mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mIsSplitImpulseActive(true), + mIsWarmStartingActive(false), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -64,9 +67,21 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { mNbContactManifolds = island->getNbContactManifolds(); + mNbFrictionConstraints = 0; + mNbPenetrationConstraints = 0; + + // TODO : Try to do faster allocation here mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; assert(mContactConstraints != nullptr); + // TODO : Count exactly the number of constraints to allocate here (do not reallocate each frame) + mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; + assert(mPenetrationConstraints != nullptr); + + // TODO : Do not reallocate each frame) + mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; + assert(mFrictionConstraints != nullptr); + // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifold(); for (uint i=0; isecond; + uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + + mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1; + mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2; + // 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 - 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); + mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse; + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse; + //internalManifold.nbContacts = externalManifold->getNbContactPoints(); + decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); + mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); internalManifold.externalContactManifold = externalManifold; - internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + + bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 = Vector3::zero(); - internalManifold.frictionPointBody2 = Vector3::zero(); + //if (mIsSolveFrictionAtContactManifoldCenterActive) { + mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero(); + mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero(); + mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); + //} + + // 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(); } + 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); + 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].restitutionFactor = restitutionFactor; + mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; + // Get the contact point on the two bodies 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(); + mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1; + mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2; + + //mPenetrationConstraints[penConstIndex].externalContact = externalContact; + mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal(); + mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth(); + //mPenetrationConstraints[penConstIndex].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); + //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1(); + //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2(); + mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0; + //mPenetrationConstraints[penConstIndex].friction1Impulse = 0.0; + //mPenetrationConstraints[penConstIndex].friction2Impulse = 0.0; + //mPenetrationConstraints[penConstIndex].rollingResistanceImpulse = Vector3::zero(); // 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 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / + decimal massPenetration = mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 + mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 + + ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 * mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r1)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal) + + ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 * mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r2)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); + massPenetration > decimal(0.0) ? mPenetrationConstraints[mNbPenetrationConstraints].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 - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); + mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; + mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN; } // If the warm starting of the contact solver is active if (mIsWarmStartingActive) { // Get the cached accumulated impulses from the previous step - contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); + mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = externalContact->getPenetrationImpulse(); } // Initialize the split impulses to zero - contactPoint.penetrationSplitImpulse = 0.0; + mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0; // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal += contactPoint.normal; - } - } + //if (mIsSolveFrictionAtContactManifoldCenterActive) { + mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal; + //} - // 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(); + mNbPenetrationConstraints++; + nbContacts++; } // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + //if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal.normalize(); + //mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); + mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts; + mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 /= nbContacts; + mFrictionConstraints[mNbFrictionConstraints].r1Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 - x1; + mFrictionConstraints[mNbFrictionConstraints].r2Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 - x2; + mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1(); + mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - - v1 - w1.cross(manifold.r1Friction); + // 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); + } + + mFrictionConstraints[mNbFrictionConstraints].normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - + v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, manifold); + computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[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 + // 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 > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass + friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass : decimal(0.0); - frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / + frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) / frictionTwistMass : decimal(0.0); - } + //} + + mNbFrictionConstraints++; } + + // Fill-in all the matrices needed to solve the LCP problem + //initializeContactConstraints(); +} + +// TODO : Delete this method +// Initialize the contact constraints before solving the system +void ContactSolver::initializeContactConstraints() { + + PROFILE("ContactSolver::initializeContactConstraints()"); + + // For each contact constraint + //for (uint c=0; c 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 +// contactPoint.restitutionBias = 0.0; +// decimal deltaVDotN = deltaV.dot(contactPoint.normal); +// if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { +// 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 +// contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); +// contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); +// contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); +// contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); +// } + +// // Initialize the split impulses to zero +// contactPoint.penetrationSplitImpulse = 0.0; + +// // If we solve the friction constraints at the center of the contact manifold +// if (mIsSolveFrictionAtContactManifoldCenterActive) { +// manifold.normal += contactPoint.normal; +// } + //} + +// // 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(); +// } + + // If we solve the friction constraints at the center of the contact manifold + //if (mIsSolveFrictionAtContactManifoldCenterActive) { + +// manifold.normal.normalize(); + +// Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - +// v1 - w1.cross(manifold.r1Friction); + +// // Compute the friction vectors +// computeFrictionVectors(deltaVFrictionPoint, manifold); + +// // 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); + //} + //} } // Warm start the solver. @@ -333,6 +467,9 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { + /* + PROFILE("ContactSolver::warmStart()"); + // Check that warm starting is active if (!mIsWarmStartingActive) return; @@ -498,285 +635,516 @@ void ContactSolver::warmStart() { contactManifold.rollingResistanceImpulse = Vector3::zero(); } } + */ } -// Solve the contacts -void ContactSolver::solve() { +// Reset the total penetration impulse of friction constraints +void ContactSolver::resetTotalPenetrationImpulse() { - PROFILE("ContactSolver::solve()"); + for (uint i=0; i SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(mPenetrationConstraints[i].penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + mPenetrationConstraints[i].restitutionBias; - // --------- Penetration --------- // - - // Compute J*v - 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 - 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 - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); - - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); - - 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); - } - } + // 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; - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + // Add the penetration impulse to the total impulse of the corresponding friction constraint + mFrictionConstraints[mPenetrationConstraints[i].indexFrictionConstraint].totalPenetrationImpulse += mPenetrationConstraints[i].penetrationImpulse; - // ------ First friction constraint at the center of the contact manifol ------ // + // 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); - // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + // 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); - // 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; + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { - // 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); + // 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)); + deltaLambda = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // 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); - // ------ 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); - } + // 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() { + + // TODO : Check that the FrictionConstraint struct only contains variables that are + // used in this method, nothing more + + PROFILE("ContactSolver::solveFrictionConstraints()"); + + for (uint i=0; i 0) { + + // Compute J*v + const Vector3 JvRolling = w2 - w1; + + // 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; + + // Compute the impulse P=J^T * lambda + angularImpulseBody1 = -deltaLambdaRolling; + angularImpulseBody2 = deltaLambdaRolling; + + // Update the velocities of the body 1 by applying the impulse P + w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 1 by applying the impulse P + w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2; + } + } +} + +// Solve the contacts +//void ContactSolver::solve() { + +// PROFILE("ContactSolver::solve()"); + +// decimal deltaLambda; +// decimal lambdaTemp; + +// // For each contact manifold +// for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * +// max(0.0f, float(contactPoint.penetrationDepth - SLOP)); +// decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + +// // Compute the Lagrange multiplier lambda +// 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 +// const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, +// contactPoint); + +// // Apply the impulse to the bodies of the constraint +// applyImpulse(impulsePenetration, contactManifold); + +// 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); +// } +// } +// } +//} + // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolver::storeImpulses() { + /* // For each contact manifold for (uint c=0; csetFrictionVector1(manifold.frictionVector1); manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); } + */ } +/* // Apply an impulse to the two bodies of a constraint void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold) { + PROFILE("ContactSolver::applyImpulse()"); + // Update the velocities of the body 1 by applying the impulse P mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; @@ -820,7 +1192,9 @@ void ContactSolver::applyImpulse(const Impulse& impulse, 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) { @@ -837,46 +1211,48 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse, mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } +*/ +// 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 ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointSolver& contactPoint) const { +//void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, +// ContactPointSolver& contactPoint) const { - assert(contactPoint.normal.length() > 0.0); +// assert(contactPoint.normal.length() > 0.0); - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; +// // Compute the velocity difference vector in the tangential plane +// Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; +// Vector3 tangentVelocity = deltaVelocity - normalVelocity; - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { +// // 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 - contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { +// // Compute the first friction vector in the direction of the tangent +// // velocity difference +// contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; +// } +// else { - // Get any orthogonal vector to the normal as the first friction vector - contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); - } +// // Get any orthogonal vector to the normal as the first friction vector +// contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); +// } - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); -} +// // The second friction vector is computed by the cross product of the firs +// // friction vector and the contact normal +// 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 { + FrictionConstraint& frictionConstraint) const { - assert(contact.normal.length() > 0.0); + assert(frictionConstraint.normal.length() > MACHINE_EPSILON); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 normalVelocity = deltaVelocity.dot(frictionConstraint.normal) * frictionConstraint.normal; Vector3 tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero @@ -885,17 +1261,17 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Compute the first friction vector in the direction of the tangent // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + frictionConstraint.frictionVector1 = tangentVelocity / lengthTangenVelocity; } else { // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + frictionConstraint.frictionVector1 = frictionConstraint.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(); + frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit(); } // Clean up the constraint solver @@ -905,4 +1281,14 @@ void ContactSolver::cleanup() { delete[] mContactConstraints; mContactConstraints = nullptr; } + + if (mPenetrationConstraints != nullptr) { + delete[] mPenetrationConstraints; + mPenetrationConstraints = nullptr; + } + + if (mFrictionConstraints != nullptr) { + delete[] mFrictionConstraints; + mFrictionConstraints = nullptr; + } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index ae2005f4..ef0087a0 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -39,7 +39,6 @@ /// ReactPhysics3D namespace namespace reactphysics3d { - // Class Contact Solver /** * This class represents the contact solver that is used to solve rigid bodies contacts. @@ -113,6 +112,156 @@ 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; + + /// Mix of the restitution factor for two bodies + decimal restitutionFactor; + + /// 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; + }; + + 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; + + /// Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody1; + + /// Point on body 2 where to apply the friction constraints + Vector3 frictionPointBody2; + + /// 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 rolling resistance impulse + Vector3 rollingResistanceImpulse; + + /// Rolling resistance factor between the two bodies + decimal rollingResistanceFactor; + + /// Mix friction coefficient for the two bodies + decimal frictionCoefficient; + + /// First friction vector in the tangent plane + Vector3 frictionVector1; + + /// Second friction vector in the tangent plane + Vector3 frictionVector2; + + /// Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector1; + + /// Old 2nd friction direction at contact manifold center + Vector3 oldFrictionVector2; + + /// 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; + + /// Total of the all the corresponding penetration impulses + decimal totalPenetrationImpulse; + + /// Inverse of the matrix K for the 1st friction + decimal inverseFriction1Mass; + + /// Inverse of the matrix K for the 2nd friction + decimal inverseFriction2Mass; + + /// Matrix K for the twist friction constraint + decimal inverseTwistFrictionMass; + + /// Matrix K for the rolling resistance constraint + Matrix3x3 inverseRollingResistance; + + /// Inverse of the mass of body 1 + decimal massInverseBody1; + + /// Inverse of the mass of body 2 + decimal massInverseBody2; + + /// Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody1; + + /// Inverse inertia tensor of body 2 + Matrix3x3 inverseInertiaTensorBody2; + }; + // Structure ContactPointSolver /** * Contact solver internal data structure that to store all the @@ -120,6 +269,30 @@ class ContactSolver { */ struct ContactPointSolver { + /// 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 + decimal massInverseBody2; + + /// Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody1; + + /// Inverse inertia tensor of body 2 + Matrix3x3 inverseInertiaTensorBody2; + + /// Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody1; + + /// Point on body 2 where to apply the friction constraints + Vector3 frictionPointBody2; + /// Accumulated normal impulse decimal penetrationImpulse; @@ -139,10 +312,10 @@ class ContactSolver { Vector3 normal; /// First friction vector in the tangent plane - Vector3 frictionVector1; + //Vector3 frictionVector1; /// Second friction vector in the tangent plane - Vector3 frictionVector2; + //Vector3 frictionVector2; /// Old first friction vector in the tangent plane Vector3 oldFrictionVector1; @@ -157,22 +330,22 @@ class ContactSolver { Vector3 r2; /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + //Vector3 r1CrossT1; /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + //Vector3 r1CrossT2; /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + //Vector3 r2CrossT1; /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + //Vector3 r2CrossT2; /// Cross product of r1 with the contact normal - Vector3 r1CrossN; + //Vector3 r1CrossN; /// Cross product of r2 with the contact normal - Vector3 r2CrossN; + //Vector3 r2CrossN; /// Penetration depth decimal penetrationDepth; @@ -181,7 +354,7 @@ class ContactSolver { decimal restitutionBias; /// Inverse of the matrix K for the penenetration - decimal inversePenetrationMass; + //decimal inversePenetrationMass; /// Inverse of the matrix K for the 1st friction decimal inverseFriction1Mass; @@ -204,40 +377,40 @@ class ContactSolver { struct ContactManifoldSolver { /// Index of body 1 in the constraint solver - uint indexBody1; + //uint indexBody1; /// Index of body 2 in the constraint solver - uint indexBody2; + //uint indexBody2; /// Inverse of the mass of body 1 - decimal massInverseBody1; + //decimal massInverseBody1; // Inverse of the mass of body 2 - decimal massInverseBody2; + //decimal massInverseBody2; /// Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody1; + //Matrix3x3 inverseInertiaTensorBody1; /// Inverse inertia tensor of body 2 - Matrix3x3 inverseInertiaTensorBody2; + //Matrix3x3 inverseInertiaTensorBody2; /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; + //ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; /// Number of contact points - uint nbContacts; + //uint nbContacts; /// True if the body 1 is of type dynamic - bool isBody1DynamicType; + //bool isBody1DynamicType; /// True if the body 2 is of type dynamic - bool isBody2DynamicType; + //bool isBody2DynamicType; /// Mix of the restitution factor for two bodies - decimal restitutionFactor; + //decimal restitutionFactor; /// Mix friction coefficient for the two bodies - decimal frictionCoefficient; + //decimal frictionCoefficient; /// Rolling resistance factor between the two bodies decimal rollingResistanceFactor; @@ -248,67 +421,67 @@ class ContactSolver { // - Variables used when friction constraints are apply at the center of the manifold-// /// Average normal vector of the contact manifold - Vector3 normal; + //Vector3 normal; /// Point on body 1 where to apply the friction constraints - Vector3 frictionPointBody1; + //Vector3 frictionPointBody1; /// Point on body 2 where to apply the friction constraints - Vector3 frictionPointBody2; + //Vector3 frictionPointBody2; /// R1 vector for the friction constraints - Vector3 r1Friction; + //Vector3 r1Friction; /// R2 vector for the friction constraints - Vector3 r2Friction; + //Vector3 r2Friction; /// Cross product of r1 with 1st friction vector - Vector3 r1CrossT1; + //Vector3 r1CrossT1; /// Cross product of r1 with 2nd friction vector - Vector3 r1CrossT2; + //Vector3 r1CrossT2; /// Cross product of r2 with 1st friction vector - Vector3 r2CrossT1; + //Vector3 r2CrossT1; /// Cross product of r2 with 2nd friction vector - Vector3 r2CrossT2; + //Vector3 r2CrossT2; /// Matrix K for the first friction constraint - decimal inverseFriction1Mass; + //decimal inverseFriction1Mass; /// Matrix K for the second friction constraint - decimal inverseFriction2Mass; + //decimal inverseFriction2Mass; /// Matrix K for the twist friction constraint - decimal inverseTwistFrictionMass; + //decimal inverseTwistFrictionMass; /// Matrix K for the rolling resistance constraint - Matrix3x3 inverseRollingResistance; + //Matrix3x3 inverseRollingResistance; /// First friction direction at contact manifold center - Vector3 frictionVector1; + //Vector3 frictionVector1; /// Second friction direction at contact manifold center - Vector3 frictionVector2; + //Vector3 frictionVector2; /// Old 1st friction direction at contact manifold center - Vector3 oldFrictionVector1; + //Vector3 oldFrictionVector1; /// Old 2nd friction direction at contact manifold center - Vector3 oldFrictionVector2; + //Vector3 oldFrictionVector2; /// First friction direction impulse at manifold center - decimal friction1Impulse; + //decimal friction1Impulse; /// Second friction direction impulse at manifold center - decimal friction2Impulse; + //decimal friction2Impulse; /// Twist friction impulse at contact manifold center - decimal frictionTwistImpulse; + //decimal frictionTwistImpulse; /// Rolling resistance impulse - Vector3 rollingResistanceImpulse; + //Vector3 rollingResistanceImpulse; }; // -------------------- Constants --------------------- // @@ -336,6 +509,14 @@ class ContactSolver { /// Contact constraints ContactManifoldSolver* mContactConstraints; + PenetrationConstraint* mPenetrationConstraints; + + FrictionConstraint* mFrictionConstraints; + + uint mNbPenetrationConstraints; + + uint mNbFrictionConstraints; + /// Number of contact constraints uint mNbContactManifolds; @@ -364,11 +545,11 @@ class ContactSolver { 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, @@ -381,29 +562,30 @@ 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, - ContactManifoldSolver& contactPoint) const; + FrictionConstraint& frictionConstraint) const; /// Compute a penetration constraint impulse - const Impulse computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; +// const Impulse computePenetrationImpulse(decimal deltaLambda, +// const PenetrationConstraint& constraint) const; /// Compute the first friction constraint impulse const Impulse computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + const FrictionConstraint& contactPoint) const; /// Compute the second friction constraint impulse const Impulse computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; + const FrictionConstraint& contactPoint) const; public: @@ -434,7 +616,16 @@ class ContactSolver { void storeImpulses(); /// Solve the contacts - void solve(); + //void solve(); + + /// Reset the total penetration impulse of friction constraints + void resetTotalPenetrationImpulse(); + + /// Solve the penetration constraints + void solvePenetrationConstraints(); + + /// Solve the friction constraints + void solveFrictionConstraints(); /// Return true if the split impulses position correction technique is used for contacts bool isSplitImpulseActive() const; @@ -502,7 +693,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 sqrt(body1->getMaterial().getFrictionCoefficient() * + return std::sqrt(body1->getMaterial().getFrictionCoefficient() * body2->getMaterial().getFrictionCoefficient()); } @@ -513,16 +704,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, } // Compute a penetration constraint impulse -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); -} +//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); +//} // Compute the first friction constraint impulse inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) + const FrictionConstraint& contactPoint) const { return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, @@ -532,7 +723,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, // Compute the second friction constraint impulse inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) + const FrictionConstraint& contactPoint) const { return Impulse(-contactPoint.frictionVector2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 2f601edd..4bd7aa19 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -353,6 +353,8 @@ 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, @@ -398,7 +400,13 @@ void DynamicsWorld::solveContactsAndConstraints() { } // Solve the contacts - if (isContactsToSolve) mContactSolver.solve(); + if (isContactsToSolve) { + + mContactSolver.resetTotalPenetrationImpulse(); + + mContactSolver.solvePenetrationConstraints(); + mContactSolver.solveFrictionConstraints(); + } } // Cache the lambda values in order to use them in the next diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index ed29a37f..ab2d126d 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -65,17 +65,17 @@ Vector3 Vector3::getOneUnitOrthogonalVector() const { assert(length() > MACHINE_EPSILON); // Get the minimum element of the vector - Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); + Vector3 vectorAbs(std::fabs(x), std::fabs(y), std::fabs(z)); int minElement = vectorAbs.getMinAxis(); if (minElement == 0) { - return Vector3(0.0, -z, y) / sqrt(y*y + z*z); + return Vector3(0.0, -z, y) / std::sqrt(y*y + z*z); } else if (minElement == 1) { - return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); + return Vector3(-z, 0.0, x) / std::sqrt(x*x + z*z); } else { - return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); + return Vector3(-y, x, 0.0) / std::sqrt(x*x + y*y); } }