From e069a25f08bfee44b84ed88ab0b7b65dd7e0b4b7 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 10 Sep 2016 11:18:52 +0200 Subject: [PATCH 01/19] 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); } } From b4f13308de720524e91f07a03c82744d59b6eba5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 13 Sep 2016 22:58:17 +0200 Subject: [PATCH 02/19] Optimize warmstarting in contact solver --- src/engine/ContactSolver.cpp | 141 +++++++++++++++++++++++++++++++++-- src/engine/ContactSolver.h | 24 ++++++ src/engine/DynamicsWorld.cpp | 4 +- src/engine/Island.h | 4 +- 4 files changed, 164 insertions(+), 9 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 22359b10..4e2e0eec 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -38,15 +38,13 @@ 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), mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(false), mIsSplitImpulseActive(true), + mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -83,7 +81,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { assert(mFrictionConstraints != nullptr); // For each contact manifold of the island - ContactManifold** contactManifolds = island->getContactManifold(); + ContactManifold** contactManifolds = island->getContactManifolds(); for (uint i=0; imCenterOfMassWorld; @@ -131,6 +130,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); internalManifold.externalContactManifold = externalManifold; + mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; @@ -167,6 +167,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor; mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; + mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact; // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); @@ -178,7 +179,10 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { //mPenetrationConstraints[penConstIndex].externalContact = externalContact; mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal(); mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth(); - //mPenetrationConstraints[penConstIndex].isRestingContact = externalContact->getIsRestingContact(); + mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact(); + + mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact; + externalContact->setIsRestingContact(true); //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1(); //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2(); @@ -467,9 +471,116 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { - /* PROFILE("ContactSolver::warmStart()"); + // Penetration constraints + for (uint i=0; isetPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse); + + } + + // Friction constraints + for (uint i=0; isetFrictionImpulse1(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 each contact manifold for (uint c=0; c Date: Fri, 16 Sep 2016 20:02:38 +0200 Subject: [PATCH 03/19] Fix issue in contact solver --- src/engine/ContactSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 4e2e0eec..a0db1119 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -832,7 +832,7 @@ void ContactSolver::solvePenetrationConstraints() { mPenetrationConstraints[i].penetrationSplitImpulse = std::max( mPenetrationConstraints[i].penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambda = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit; + 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; From 92460791e651bf62216a14992c4bfb7da5926952 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 21 Sep 2016 22:01:14 +0200 Subject: [PATCH 04/19] Refactor PoolAllocator and add SingleFrameAllocator --- ...{MemoryAllocator.cpp => PoolAllocator.cpp} | 16 ++-- .../{MemoryAllocator.h => PoolAllocator.h} | 12 +-- src/memory/SingleFrameAllocator.cpp | 80 +++++++++++++++++++ src/memory/SingleFrameAllocator.h | 76 ++++++++++++++++++ 4 files changed, 170 insertions(+), 14 deletions(-) rename src/memory/{MemoryAllocator.cpp => PoolAllocator.cpp} (95%) rename src/memory/{MemoryAllocator.h => PoolAllocator.h} (96%) create mode 100644 src/memory/SingleFrameAllocator.cpp create mode 100644 src/memory/SingleFrameAllocator.h diff --git a/src/memory/MemoryAllocator.cpp b/src/memory/PoolAllocator.cpp similarity index 95% rename from src/memory/MemoryAllocator.cpp rename to src/memory/PoolAllocator.cpp index 46c47e14..dc1306bc 100644 --- a/src/memory/MemoryAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -24,19 +24,19 @@ ********************************************************************************/ // Libraries -#include "MemoryAllocator.h" +#include "PoolAllocator.h" #include #include using namespace reactphysics3d; // Initialization of static variables -bool MemoryAllocator::isMapSizeToHeadIndexInitialized = false; -size_t MemoryAllocator::mUnitSizes[NB_HEAPS]; -int MemoryAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; +bool PoolAllocator::isMapSizeToHeadIndexInitialized = false; +size_t PoolAllocator::mUnitSizes[NB_HEAPS]; +int PoolAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1]; // Constructor -MemoryAllocator::MemoryAllocator() { +PoolAllocator::PoolAllocator() { // Allocate some memory to manage the blocks mNbAllocatedMemoryBlocks = 64; @@ -78,7 +78,7 @@ MemoryAllocator::MemoryAllocator() { } // Destructor -MemoryAllocator::~MemoryAllocator() { +PoolAllocator::~PoolAllocator() { // Release the memory allocated for each block for (uint i=0; i @@ -33,14 +33,14 @@ /// ReactPhysics3D namespace namespace reactphysics3d { -// Class MemoryAllocator +// Class PoolAllocator /** * This class is used to efficiently allocate memory on the heap. * It allows us to allocate small blocks of memory (smaller or equal to 1024 bytes) * efficiently. This implementation is inspired by the small block allocator * described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp */ -class MemoryAllocator { +class PoolAllocator { private : @@ -126,10 +126,10 @@ class MemoryAllocator { // -------------------- Methods -------------------- // /// Constructor - MemoryAllocator(); + PoolAllocator(); /// Destructor - ~MemoryAllocator(); + ~PoolAllocator(); /// Allocate memory of a given size (in bytes) and return a pointer to the /// allocated memory. diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp new file mode 100644 index 00000000..a2e436cd --- /dev/null +++ b/src/memory/SingleFrameAllocator.cpp @@ -0,0 +1,80 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "SingleFrameAllocator.h" +#include +#include + +using namespace reactphysics3d; + +// Constructor +SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes) + : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) { + + // Allocate a whole block of memory at the beginning + mMemoryBufferStart = static_cast(malloc(mTotalSizeBytes)); + assert(mMemoryBufferStart != nullptr); +} + +// Destructor +SingleFrameAllocator::~SingleFrameAllocator() { + + // Release the memory allocated at the beginning + free(mMemoryBufferStart); +} + + +// Allocate memory of a given size (in bytes) and return a pointer to the +// allocated memory. +void* SingleFrameAllocator::allocate(size_t size) { + + // Check that there is enough remaining memory in the buffer + if (static_cast(mCurrentOffset) + size > mTotalSizeBytes) { + + // This should never occur. If it does, you must increase the initial + // size of memory of this allocator + assert(false); + + // Return null + return nullptr; + } + + // Next available memory location + void* nextAvailableMemory = mMemoryBufferStart + mCurrentOffset; + + // Increment the offset + mCurrentOffset += size; + + // Return the next available memory location + return nextAvailableMemory; +} + +// Reset the marker of the current allocated memory +void SingleFrameAllocator::reset() { + + // Reset the current offset at the beginning of the block + mCurrentOffset = 0; +} diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h new file mode 100644 index 00000000..e3151f0a --- /dev/null +++ b/src/memory/SingleFrameAllocator.h @@ -0,0 +1,76 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H +#define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H + +// Libraries +#include +#include "configuration.h" + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class SingleFrameAllocator +/** + * This class represent a memory allocator used to efficiently allocate + * memory on the heap that is used during a single frame. + */ +class SingleFrameAllocator { + + private : + + // -------------------- Attributes -------------------- // + + /// Total size (in bytes) of memory of the allocator + size_t mTotalSizeBytes; + + /// Pointer to the beginning of the allocated memory block + char* mMemoryBufferStart; + + /// Pointer to the next available memory location in the buffer + int mCurrentOffset; + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SingleFrameAllocator(size_t totalSizeBytes); + + /// Destructor + ~SingleFrameAllocator(); + + /// Allocate memory of a given size (in bytes) + void* allocate(size_t size); + + /// Reset the marker of the current allocated memory + void reset(); + +}; + +} + +#endif From e014f00afc71bf62e78d02fba1e2fea3e957fbb3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 21 Sep 2016 22:03:45 +0200 Subject: [PATCH 05/19] Refactor memory allocator and refactor contact solver --- CMakeLists.txt | 6 +- src/body/CollisionBody.cpp | 10 +- src/body/CollisionBody.h | 2 +- src/body/RigidBody.cpp | 4 +- src/body/RigidBody.h | 4 +- src/collision/CollisionDetection.cpp | 22 +- src/collision/CollisionDetection.h | 8 +- src/collision/ContactManifold.cpp | 2 +- src/collision/ContactManifold.h | 6 +- src/collision/ContactManifoldSet.cpp | 2 +- src/collision/ContactManifoldSet.h | 4 +- src/collision/narrowphase/CollisionDispatch.h | 2 +- .../narrowphase/DefaultCollisionDispatch.cpp | 2 +- .../narrowphase/DefaultCollisionDispatch.h | 2 +- src/collision/narrowphase/EPA/EPAAlgorithm.h | 8 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 4 +- .../narrowphase/GJK/VoronoiSimplex.cpp | 1 - .../narrowphase/NarrowPhaseAlgorithm.cpp | 2 +- .../narrowphase/NarrowPhaseAlgorithm.h | 6 +- src/collision/shapes/CollisionShape.h | 2 +- src/configuration.h | 3 + src/engine/CollisionWorld.cpp | 6 +- src/engine/CollisionWorld.h | 6 +- src/engine/ContactSolver.cpp | 805 ++---------------- src/engine/ContactSolver.h | 249 +----- src/engine/DynamicsWorld.cpp | 103 ++- src/engine/DynamicsWorld.h | 3 + src/engine/Island.cpp | 2 +- src/engine/Island.h | 6 +- src/engine/OverlappingPair.cpp | 2 +- src/engine/OverlappingPair.h | 2 +- 31 files changed, 201 insertions(+), 1085 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e85693d7..69f20522 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,8 +174,10 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" - "src/memory/MemoryAllocator.h" - "src/memory/MemoryAllocator.cpp" + "src/memory/PoolAllocator.h" + "src/memory/PoolAllocator.cpp" + "src/memory/SingleFrameAllocator.h" + "src/memory/SingleFrameAllocator.cpp" "src/memory/Stack.h" ) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index daa4a71e..db22f606 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -70,7 +70,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, const Transform& transform) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, decimal(1)); @@ -116,7 +116,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -136,7 +136,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } elementToRemove->~ProxyShape(); - mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -162,7 +162,7 @@ void CollisionBody::removeAllCollisionShapes() { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); // Get the next element in the list current = nextElement; @@ -181,7 +181,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the current element currentElement->~ContactManifoldListElement(); - mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 5fda6958..bd5c7818 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -34,7 +34,7 @@ #include "collision/shapes/AABB.h" #include "collision/shapes/CollisionShape.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "configuration.h" /// Namespace reactphysics3d diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index ff77b228..74ee8a05 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -166,7 +166,7 @@ void RigidBody::setMass(decimal mass) { } // Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { +void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) { assert(joint != nullptr); assert(mJointsList != nullptr); @@ -214,7 +214,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, decimal mass) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, mass); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 8fb36d0e..0ffc6257 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -31,7 +31,7 @@ #include "CollisionBody.h" #include "engine/Material.h" #include "mathematics/mathematics.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -104,7 +104,7 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // /// Remove a joint from the joints list - void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint); /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7d9f6fb2..c3caa97a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -41,7 +41,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) +CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator), mWorld(world), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { @@ -189,7 +189,7 @@ void CollisionDetection::computeNarrowPhase() { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -294,7 +294,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -370,8 +370,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro shape2->getCollisionShape()->getType()); // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); + OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); assert(newPair != nullptr); #ifndef NDEBUG @@ -400,7 +400,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); } else { @@ -434,7 +434,7 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { // Create a new contact - ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) + ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactInfo); // Add the contact to the contact manifold set of the corresponding overlapping pair @@ -477,7 +477,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement1 = new (allocatedMemory1) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); @@ -485,7 +485,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement2 = new (allocatedMemory2) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); @@ -520,8 +520,8 @@ EventListener* CollisionDetection::getWorldEventListener() { } /// Return a reference to the world memory allocator -MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { - return mWorld->mMemoryAllocator; +PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { + return mWorld->mPoolAllocator; } // Called by a narrow-phase collision algorithm when a new contact has been found diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 75f278ba..0fe486c4 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -32,7 +32,7 @@ #include "engine/OverlappingPair.h" #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "constraint/ContactPoint.h" #include #include @@ -93,7 +93,7 @@ class CollisionDetection : public NarrowPhaseCallback { NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Pointer to the physics world CollisionWorld* mWorld; @@ -143,7 +143,7 @@ class CollisionDetection : public NarrowPhaseCallback { // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); + CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator); /// Destructor ~CollisionDetection() = default; @@ -220,7 +220,7 @@ class CollisionDetection : public NarrowPhaseCallback { EventListener* getWorldEventListener(); /// Return a reference to the world memory allocator - MemoryAllocator& getWorldMemoryAllocator(); + PoolAllocator& getWorldMemoryAllocator(); /// Called by a narrow-phase collision algorithm when a new contact has been found virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override; diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index cfa905ff..ff15bce6 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short normalDirectionId) + PoolAllocator& memoryAllocator, short normalDirectionId) : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index eb7716df..2f77e9b2 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -31,7 +31,7 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -126,7 +126,7 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; // -------------------- Methods -------------------- // @@ -151,7 +151,7 @@ class ContactManifold { /// Constructor ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short int normalDirectionId); + PoolAllocator& memoryAllocator, short int normalDirectionId); /// Destructor ~ContactManifold(); diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 54e3614e..26f3a66f 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds) + PoolAllocator& memoryAllocator, int nbMaxManifolds) : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), mShape2(shape2), mMemoryAllocator(memoryAllocator) { assert(nbMaxManifolds >= 1); diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index b167f506..21944e36 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -61,7 +61,7 @@ class ContactManifoldSet { ProxyShape* mShape2; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Contact manifolds of the set ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; @@ -88,7 +88,7 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds); + PoolAllocator& memoryAllocator, int nbMaxManifolds); /// Destructor ~ContactManifoldSet(); diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 8fa24f87..6e027721 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -51,7 +51,7 @@ class CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 109b5de3..f6812d74 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; /// Initialize the collision dispatch configuration void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { // Initialize the collision algorithms mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 9bf15b11..5dd07bf2 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -63,7 +63,7 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index f585ac15..05a78eea 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -34,7 +34,7 @@ #include "collision/narrowphase/NarrowPhaseAlgorithm.h" #include "mathematics/mathematics.h" #include "TriangleEPA.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include /// ReactPhysics3D namespace @@ -88,7 +88,7 @@ class EPAAlgorithm { // -------------------- Attributes -------------------- // /// Reference to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Triangle comparison operator TriangleComparison mTriangleComparison; @@ -120,7 +120,7 @@ class EPAAlgorithm { EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; /// Initalize the algorithm - void init(MemoryAllocator* memoryAllocator); + void init(PoolAllocator* memoryAllocator); /// Compute the penetration depth with EPA algorithm. bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, @@ -151,7 +151,7 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** } // Initalize the algorithm -inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) { +inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) { mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e81ee9b3..b6cce5c2 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -94,7 +94,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Initalize the algorithm virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Compute a contact info if the two bounding volumes collide. virtual void testCollision(const CollisionShapeInfo& shape1Info, @@ -110,7 +110,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { // Initalize the algorithm inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); mAlgoEPA.init(memoryAllocator); } diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp index 382ccc9b..590182ac 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -563,7 +563,6 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve if (squareDist < closestSquareDistance) { // Use it as the current closest point - closestSquareDistance = squareDist; baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]); baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]); bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2); diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp index 0fbbe5fe..782f8d6f 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp @@ -36,7 +36,7 @@ NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() } // Initalize the algorithm -void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { +void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) { mCollisionDetection = collisionDetection; mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 30b216f0..0759d138 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "body/Body.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" #include "collision/CollisionShapeInfo.h" @@ -71,7 +71,7 @@ class NarrowPhaseAlgorithm { CollisionDetection* mCollisionDetection; /// Pointer to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Overlapping pair of the bodies currently tested for collision OverlappingPair* mCurrentOverlappingPair; @@ -93,7 +93,7 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); + virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator); /// Set the current overlapping pair of bodies void setCurrentOverlappingPair(OverlappingPair* overlappingPair); diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 1e2e3f14..b33ed4f1 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -34,7 +34,7 @@ #include "mathematics/Ray.h" #include "AABB.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/configuration.h b/src/configuration.h index 4364c548..87747756 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -144,6 +144,9 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; /// least one concave collision shape. constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +/// Size (in bytes) of the single frame allocator +constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; + } #endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 68e37625..b1b004ec 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,7 +33,7 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), + : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0), mEventListener(nullptr) { } @@ -66,7 +66,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the collision body - CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) + CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); assert(collisionBody != nullptr); @@ -97,7 +97,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { mBodies.erase(collisionBody); // Free the object from the memory allocator - mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); + mPoolAllocator.release(collisionBody, sizeof(CollisionBody)); } // Return the next available body ID diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 77189e89..d04acb4e 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,7 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "EventListener.h" /// Namespace reactphysics3d @@ -72,8 +72,8 @@ class CollisionWorld { /// List of free ID for rigid bodies std::vector mFreeBodiesIDs; - /// Memory allocator - MemoryAllocator mMemoryAllocator; + /// Pool Memory allocator + PoolAllocator mPoolAllocator; /// Pointer to an event listener object EventListener* mEventListener; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index a0db1119..b271aa6e 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,55 +39,75 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP= decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) +ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& singleFrameAllocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mPenetrationConstraints(nullptr), - mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + mSingleFrameAllocator(singleFrameAllocator), + mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr), + mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } +// 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)); + //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints]; + assert(mPenetrationConstraints != nullptr); + //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; + + mFrictionConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); + //mFrictionConstraints = new FrictionConstraint[nbContactManifolds]; + assert(mFrictionConstraints != nullptr); + //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; + + // 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(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(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(); - - 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->getContactManifolds(); - for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& internalManifold = mContactConstraints[i]; - assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact @@ -100,6 +120,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { 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; @@ -129,7 +150,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; @@ -159,6 +179,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { // 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; @@ -301,168 +322,19 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { 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() { +// Solve the contact constraints of one iteration of the solve +void ContactSolver::solve() { - PROFILE("ContactSolver::initializeContactConstraints()"); - - // For each contact constraint - //for (uint c=0; c decimal(0.0)); - // ContactManifoldSolver& manifold = mContactConstraints[c]; - -// // Get the inertia tensors of both bodies -// Matrix3x3& I1 = manifold.inverseInertiaTensorBody1; -// Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; - - // If we solve the friction constraints at the center of the contact manifold -// if (mIsSolveFrictionAtContactManifoldCenterActive) { -// manifold.normal = Vector3(0.0, 0.0, 0.0); -// } - - // Get the velocities of the bodies -// const Vector3& v1 = mLinearVelocities[manifold.indexBody1]; -// const Vector3& w1 = mAngularVelocities[manifold.indexBody1]; -// const Vector3& v2 = mLinearVelocities[manifold.indexBody2]; -// const Vector3& w2 = mAngularVelocities[manifold.indexBody2]; - - // For each contact point constraint - //for (uint i=0; i 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); - //} - //} + resetTotalPenetrationImpulse(); + solvePenetrationConstraints(); + solveFrictionConstraints(); } // Warm start the solver. @@ -576,177 +448,6 @@ void ContactSolver::warmStart() { mFrictionConstraints[i].rollingResistanceImpulse.setToZero(); } } - - - /* - - - // Check that warm starting is active - if (!mIsWarmStartingActive) return; - - // For each constraint - for (uint c=0; c 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(); - } - } - - // If we solve the friction constraints at the center of the contact manifold and there is - // at least one resting contact point in the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { - - // Project the old friction impulses (with old friction vectors) into the new friction - // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); - - // ------ First friction constraint at the center of the contact manifold ------ // - - // Compute the impulse P = J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * - contactManifold.friction1Impulse; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * - contactManifold.friction1Impulse; - 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 manifold ----- // - - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; - 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 manifold ------ // - - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); - - // ------ Rolling resistance at the center of the contact manifold ------ // - - // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; - angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - else { // If it is a new contact manifold - - // Initialize the accumulated impulses to zero - contactManifold.friction1Impulse = 0.0; - contactManifold.friction2Impulse = 0.0; - contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = Vector3::zero(); - } - } - */ } // Reset the total penetration impulse of friction constraints @@ -978,288 +679,13 @@ void ContactSolver::solveFrictionConstraints() { } } -// 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() { // Penetration constraints for (uint i=0; isetPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse); - } // Friction constraints @@ -1274,105 +700,17 @@ void ContactSolver::storeImpulses() { } /* - // For each contact manifold - for (uint c=0; csetPenetrationImpulse(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); + if (mFrictionConstraints != nullptr) { + delete[] mFrictionConstraints; } */ } -/* -// 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; - 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; -} -*/ - -// 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 { - -// 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; - -// // 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 { - -// // 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(); -//} - // 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, @@ -1402,22 +740,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit(); } - -// Clean up the constraint solver -void ContactSolver::cleanup() { - - if (mContactConstraints != nullptr) { - 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 f3f5dfa7..e5d5840d 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -31,6 +31,7 @@ #include "configuration.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" +#include "memory/SingleFrameAllocator.h" #include "Island.h" #include "Impulse.h" #include @@ -278,228 +279,6 @@ class ContactSolver { bool hasAtLeastOneRestingContactPoint; }; - // Structure ContactPointSolver - /** - * Contact solver internal data structure that to store all the - * information relative to a contact point - */ - 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; - - /// Accumulated impulse in the 1st friction direction - decimal friction1Impulse; - - /// Accumulated impulse in the 2nd friction direction - decimal friction2Impulse; - - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; - - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; - - /// Normal vector of the contact - Vector3 normal; - - /// First friction vector in the tangent plane - //Vector3 frictionVector1; - - /// Second friction vector in the tangent plane - //Vector3 frictionVector2; - - /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; - - /// 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; - - /// 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; - - /// 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; - - /// Inverse of the matrix K for the 2nd friction - decimal inverseFriction2Mass; - - /// True if the contact was existing last time step - bool isRestingContact; - - /// 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 - //decimal massInverseBody2; - - /// Inverse inertia tensor of body 1 - //Matrix3x3 inverseInertiaTensorBody1; - - /// Inverse inertia tensor of body 2 - //Matrix3x3 inverseInertiaTensorBody2; - - /// Contact point constraints - //ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - - /// 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 --------------------- // /// Beta value for the penetration depth position correction without split impulses @@ -519,12 +298,12 @@ 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; - /// Contact constraints - ContactManifoldSolver* mContactConstraints; - PenetrationConstraint* mPenetrationConstraints; FrictionConstraint* mFrictionConstraints; @@ -533,9 +312,6 @@ class ContactSolver { uint mNbFrictionConstraints; - /// Number of contact constraints - uint mNbContactManifolds; - /// Array of linear velocities Vector3* mLinearVelocities; @@ -557,8 +333,8 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Initialize the contact constraints before solving the system - void initializeContactConstraints(); + /// Initialize the constraint solver for a given island + void initializeForIsland(Island* island); /// Apply an impulse to the two bodies of a constraint //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); @@ -608,13 +384,17 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex); + ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& singleFrameAllocator); /// Destructor ~ContactSolver() = default; - /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + /// Initialize the contact constraints + void init(Island** islands, uint nbIslands, decimal timeStep); + + /// Solve the contact constraints of one iteration of the solve + void solve(); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -653,9 +433,6 @@ class ContactSolver { /// the contact manifold instead of solving them at each contact point void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Clean up the constraint solver - void cleanup(); - /// Return true if warmstarting is active bool IsWarmStartingActive() const; }; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 189ef0e0..0b1110af 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -40,7 +40,8 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex), + mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), + mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), @@ -82,10 +83,10 @@ DynamicsWorld::~DynamicsWorld() { mIslands[i]->~Island(); // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); + mPoolAllocator.release(mIslands[i], sizeof(Island)); } if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); } // Release the memory allocated for the bodies velocity arrays @@ -161,6 +162,9 @@ void DynamicsWorld::update(decimal timeStep) { // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); + + // Reset the single frame memory allocator + mSingleFrameAllocator.reset(); } // Integrate position and orientation of the rigid bodies. @@ -364,27 +368,28 @@ void DynamicsWorld::solveContactsAndConstraints() { mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, mConstrainedOrientations); - // ---------- Solve velocity constraints for joints and contacts ---------- // + // Initialize the contact solver + mContactSolver.init(mIslands, mNbIslands, mTimeStep); // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + //if (!isConstraintsToSolve && !isContactsToSolve) continue; // If there are contacts in the current island - if (isContactsToSolve) { +// if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); +// // Initialize the solver +// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - if (mContactSolver.IsWarmStartingActive()) { - mContactSolver.warmStart(); - } - } +// // Warm start the contact solver +// if (mContactSolver.IsWarmStartingActive()) { +// mContactSolver.warmStart(); +// } +// } // If there are constraints if (isConstraintsToSolve) { @@ -392,32 +397,40 @@ 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]); + } } + mContactSolver.solve(); + // Solve the contacts - if (isContactsToSolve) { +// if (isContactsToSolve) { - mContactSolver.resetTotalPenetrationImpulse(); +// mContactSolver.resetTotalPenetrationImpulse(); - mContactSolver.solvePenetrationConstraints(); - mContactSolver.solveFrictionConstraints(); - } +// mContactSolver.solvePenetrationConstraints(); +// mContactSolver.solveFrictionConstraints(); +// } } // Cache the lambda values in order to use them in the next // step and cleanup the contact solver - if (isContactsToSolve) { - mContactSolver.storeImpulses(); - mContactSolver.cleanup(); - } - } +// if (isContactsToSolve) { +// mContactSolver.storeImpulses(); +// mContactSolver.cleanup(); +// } + //} + + mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints @@ -456,7 +469,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the rigid body - RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, + RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, *this, bodyID); assert(rigidBody != nullptr); @@ -497,7 +510,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mRigidBodies.erase(rigidBody); // Free the object from the memory allocator - mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); + mPoolAllocator.release(rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint @@ -515,7 +528,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(info); @@ -525,7 +538,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(info); break; @@ -534,7 +547,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(info); break; @@ -543,7 +556,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(info); break; @@ -596,8 +609,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mJoints.erase(joint); // Remove the joint from the joint list of the bodies involved in the joint - joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint); - joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); + joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint); + joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -605,7 +618,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->~Joint(); // Release the allocated memory - mMemoryAllocator.release(joint, nbBytes); + mPoolAllocator.release(joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint @@ -614,13 +627,13 @@ void DynamicsWorld::addJointToBody(Joint* joint) { assert(joint != nullptr); // Add the joint at the beginning of the linked list of joints of the first body - void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory1 = mPoolAllocator.allocate(sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, joint->mBody1->mJointsList); joint->mBody1->mJointsList = jointListElement1; // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; @@ -646,16 +659,16 @@ void DynamicsWorld::computeIslands() { mIslands[i]->~Island(); // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); + mPoolAllocator.release(mIslands[i], sizeof(Island)); } // Allocate and create the array of islands if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); } mNbIslandsCapacity = nbBodies; - mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); + mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); } mNbIslands = 0; @@ -672,7 +685,7 @@ void DynamicsWorld::computeIslands() { // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); + RigidBody** stackBodiesToVisit = (RigidBody**)mPoolAllocator.allocate(nbBytesStack); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -695,10 +708,10 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); + void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island)); mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, - mJoints.size(), mMemoryAllocator); + mJoints.size(), mPoolAllocator); // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -790,7 +803,7 @@ void DynamicsWorld::computeIslands() { } // Release the allocated memory for the stack of bodies to visit - mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); + mPoolAllocator.release(stackBodiesToVisit, nbBytesStack); } // Put bodies to sleep if needed. diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 7dc715c9..542e0f4b 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -50,6 +50,9 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Attributes -------------------- // + /// Single frame Memory allocator + SingleFrameAllocator mSingleFrameAllocator; + /// Contact solver ContactSolver mContactSolver; diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 41560bde..8cf743a3 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator) + PoolAllocator& memoryAllocator) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { diff --git a/src/engine/Island.h b/src/engine/Island.h index 512a0d1b..227a78e5 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_ISLAND_H // Libraries -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "body/RigidBody.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" @@ -64,7 +64,7 @@ class Island { uint mNbJoints; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Number of bytes allocated for the bodies array size_t mNbAllocatedBytesBodies; @@ -81,7 +81,7 @@ class Island { /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator); + PoolAllocator& memoryAllocator); /// Destructor ~Island(); diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 7efca599..48658f5c 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator) + int nbMaxContactManifolds, PoolAllocator& memoryAllocator) : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), mCachedSeparatingAxis(0.0, 1.0, 0.0) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 7995b0d2..70ee1c3f 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -63,7 +63,7 @@ class OverlappingPair { /// Constructor OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator); + int nbMaxContactManifolds, PoolAllocator& memoryAllocator); /// Destructor ~OverlappingPair() = default; From 8f4979f4a2e50419dc25a5696c83dca45932e047 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 22 Sep 2016 23:24:03 +0200 Subject: [PATCH 06/19] Allocate memory in the SingleFrameAllocator in the update() method --- src/engine/DynamicsWorld.cpp | 96 +++++++++--------------------------- src/engine/DynamicsWorld.h | 6 --- src/engine/Island.cpp | 24 +++------ src/engine/Island.h | 16 +----- 4 files changed, 34 insertions(+), 108 deletions(-) diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 0b1110af..91312546 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -49,8 +49,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), - mConstrainedOrientations(nullptr), mNbIslands(0), - mNbIslandsCapacity(0), mIslands(nullptr), mNbBodiesCapacity(0), + mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr), mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { @@ -76,29 +75,6 @@ DynamicsWorld::~DynamicsWorld() { destroyRigidBody(*itToRemove); } - // Release the memory allocated for the islands - for (uint i=0; i~Island(); - - // Release the allocated memory for the island - mPoolAllocator.release(mIslands[i], sizeof(Island)); - } - if (mNbIslandsCapacity > 0) { - mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } - - // Release the memory allocated for the bodies velocity arrays - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - delete[] mConstrainedLinearVelocities; - delete[] mConstrainedAngularVelocities; - delete[] mConstrainedPositions; - delete[] mConstrainedOrientations; - } - assert(mJoints.size() == 0); assert(mRigidBodies.size() == 0); @@ -245,31 +221,26 @@ void DynamicsWorld::updateBodiesState() { // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { + PROFILE("DynamicsWorld::initVelocityArrays()"); + // Allocate memory for the bodies velocity arrays uint nbBodies = mRigidBodies.size(); - if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { - if (mNbBodiesCapacity > 0) { - delete[] mSplitLinearVelocities; - delete[] mSplitAngularVelocities; - } - mNbBodiesCapacity = nbBodies; - // TODO : Use better memory allocation here - mSplitLinearVelocities = new Vector3[mNbBodiesCapacity]; - mSplitAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity]; - mConstrainedPositions = new Vector3[mNbBodiesCapacity]; - mConstrainedOrientations = new Quaternion[mNbBodiesCapacity]; - assert(mSplitLinearVelocities != nullptr); - assert(mSplitAngularVelocities != nullptr); - assert(mConstrainedLinearVelocities != nullptr); - assert(mConstrainedAngularVelocities != nullptr); - assert(mConstrainedPositions != nullptr); - assert(mConstrainedOrientations != nullptr); - } + + mSplitLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); + mSplitAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); + mConstrainedLinearVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); + mConstrainedAngularVelocities = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); + mConstrainedPositions = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3))); + mConstrainedOrientations = static_cast(mSingleFrameAllocator.allocate(nbBodies * sizeof(Quaternion))); + assert(mSplitLinearVelocities != nullptr); + assert(mSplitAngularVelocities != nullptr); + assert(mConstrainedLinearVelocities != nullptr); + assert(mConstrainedAngularVelocities != nullptr); + assert(mConstrainedPositions != nullptr); + assert(mConstrainedOrientations != nullptr); // Reset the velocities arrays - for (uint i=0; i~Island(); - - // Release the allocated memory for the island - mPoolAllocator.release(mIslands[i], sizeof(Island)); - } - - // Allocate and create the array of islands - if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { - if (mNbIslandsCapacity > 0) { - mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); - } - mNbIslandsCapacity = nbBodies; - mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); - } + // Allocate and create the array of islands pointer. This memory is allocated + // in the single frame allocator + mIslands = static_cast(mSingleFrameAllocator.allocate(sizeof(Island*) * nbBodies)); mNbIslands = 0; int nbContactManifolds = 0; @@ -685,7 +641,7 @@ void DynamicsWorld::computeIslands() { // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = (RigidBody**)mPoolAllocator.allocate(nbBytesStack); + RigidBody** stackBodiesToVisit = static_cast(mSingleFrameAllocator.allocate(nbBytesStack)); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -708,10 +664,9 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island)); - mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, - nbContactManifolds, - mJoints.size(), mPoolAllocator); + void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island)); + mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(), + mSingleFrameAllocator); // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -801,9 +756,6 @@ void DynamicsWorld::computeIslands() { mNbIslands++; } - - // Release the allocated memory for the stack of bodies to visit - mPoolAllocator.release(stackBodiesToVisit, nbBytesStack); } // Put bodies to sleep if needed. diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 542e0f4b..dec383a1 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -109,15 +109,9 @@ class DynamicsWorld : public CollisionWorld { /// Number of islands in the world uint mNbIslands; - /// Current allocated capacity for the islands - uint mNbIslandsCapacity; - /// Array with all the islands of awaken bodies Island** mIslands; - /// Current allocated capacity for the bodies - uint mNbBodiesCapacity; - /// Sleep linear velocity threshold decimal mSleepLinearVelocity; diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 8cf743a3..19eef304 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -29,26 +29,18 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - PoolAllocator& memoryAllocator) +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), - mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { + mNbContactManifolds(0), mNbJoints(0) { - // Allocate memory for the arrays - mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; - mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); - mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; - mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( - mNbAllocatedBytesContactManifolds); - mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints; - mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); + // Allocate memory for the arrays on the single frame allocator + mBodies = static_cast(allocator.allocate(sizeof(RigidBody*) * nbMaxBodies)); + mContactManifolds = static_cast(allocator.allocate(sizeof(ContactManifold*) * nbMaxContactManifolds)); + mJoints = static_cast(allocator.allocate(sizeof(Joint*) * nbMaxJoints)); } // Destructor Island::~Island() { - - // Release the memory of the arrays - mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); - mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); - mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); + // This destructor is never called because memory is allocated on the + // single frame allocator } diff --git a/src/engine/Island.h b/src/engine/Island.h index 227a78e5..5f370004 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_ISLAND_H // Libraries -#include "memory/PoolAllocator.h" +#include "memory/SingleFrameAllocator.h" #include "body/RigidBody.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" @@ -63,25 +63,13 @@ class Island { /// Current number of joints in the island uint mNbJoints; - /// Reference to the memory allocator - PoolAllocator& mMemoryAllocator; - - /// Number of bytes allocated for the bodies array - size_t mNbAllocatedBytesBodies; - - /// Number of bytes allocated for the contact manifolds array - size_t mNbAllocatedBytesContactManifolds; - - /// Number of bytes allocated for the joints array - size_t mNbAllocatedBytesJoints; - public: // -------------------- Methods -------------------- // /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - PoolAllocator& memoryAllocator); + SingleFrameAllocator& allocator); /// Destructor ~Island(); From c59781519167421c05525e0b48586b16df64f5e8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 26 Sep 2016 22:51:30 +0200 Subject: [PATCH 07/19] Remove unecessary variables in constraints and cache inverse inertia world tensor of bodies --- src/body/RigidBody.cpp | 17 ++++ src/body/RigidBody.h | 23 +++-- src/engine/ContactSolver.cpp | 163 +++++++++++++---------------------- src/engine/ContactSolver.h | 13 --- src/engine/DynamicsWorld.cpp | 3 + 5 files changed, 97 insertions(+), 122 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 74ee8a05..6e6a55e6 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -46,6 +46,9 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde // Compute the inverse mass mMassInverse = decimal(1.0) / mInitMass; + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Destructor @@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) { mMassInverse = decimal(0.0); mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); + mInertiaTensorInverseWorld.setToZero(); } else { // If it is a dynamic body mMassInverse = decimal(1.0) / mInitMass; mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Awake the body @@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { // Compute the inverse local inertia tensor mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); } // Set the local center of mass of the body (in local-space coordinates) @@ -314,6 +324,9 @@ void RigidBody::setTransform(const Transform& transform) { // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Update the broad-phase state of the body updateBroadPhaseState(); } @@ -326,6 +339,7 @@ void RigidBody::recomputeMassInformation() { mMassInverse = decimal(0.0); mInertiaTensorLocal.setToZero(); mInertiaTensorLocalInverse.setToZero(); + mInertiaTensorInverseWorld.setToZero(); mCenterOfMassLocal.setToZero(); // If it is STATIC or KINEMATIC body @@ -386,6 +400,9 @@ void RigidBody::recomputeMassInformation() { // Compute the local inverse inertia tensor mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse(); + // Update the world inverse inertia tensor + updateInertiaTensorInverseWorld(); + // Update the linear velocity of the center of mass mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 0ffc6257..a38b53d5 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -83,6 +83,9 @@ class RigidBody : public CollisionBody { /// Inverse of the inertia tensor of the body Matrix3x3 mInertiaTensorLocalInverse; + /// Inverse of the world inertia tensor of the body + Matrix3x3 mInertiaTensorInverseWorld; + /// Inverse of the mass of the body decimal mMassInverse; @@ -112,6 +115,9 @@ class RigidBody : public CollisionBody { /// Update the broad-phase state for this body (because it has moved for instance) virtual void updateBroadPhaseState() const override; + /// Update the world inverse inertia tensor of the body + void updateInertiaTensorInverseWorld(); + public : // -------------------- Methods -------------------- // @@ -291,12 +297,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { */ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { - // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE - // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES - // Compute and return the inertia tensor in world coordinates - return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse * - mTransform.getOrientation().getMatrix().getTranspose(); + return mInertiaTensorInverseWorld; +} + +// Update the world inverse inertia tensor of the body +/// The inertia tensor I_w in world coordinates is computed with the +/// local inverse inertia tensor I_b^-1 in body coordinates +/// by I_w = R * I_b^-1 * R^T +/// where R is the rotation matrix (and R^T its transpose) of the +/// current orientation quaternion of the body +inline void RigidBody::updateInertiaTensorInverseWorld() { + Matrix3x3 orientation = mTransform.getOrientation().getMatrix(); + mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose(); } // Return true if the gravity needs to be applied to this rigid body diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index b271aa6e..d2218136 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,7 +36,7 @@ using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP= decimal(0.01); +const decimal ContactSolver::SLOP = decimal(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, @@ -74,14 +74,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { // 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)); - //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints]; assert(mPenetrationConstraints != nullptr); - //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; mFrictionConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); - //mFrictionConstraints = new FrictionConstraint[nbContactManifolds]; assert(mFrictionConstraints != nullptr); - //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; // For each island of the world for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { @@ -146,23 +142,17 @@ void ContactSolver::initializeForIsland(Island* island) { // contact manifold 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); mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; - //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) { - mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero(); - mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero(); - mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero(); - //} + Vector3 frictionPointBody1; + Vector3 frictionPointBody2; + mFrictionConstraints[mNbFrictionConstraints].normal.setToZero(); // Compute the inverse K matrix for the rolling resistance constraint mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero(); @@ -186,7 +176,6 @@ void ContactSolver::initializeForIsland(Island* island) { mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse; mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse; - mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor; mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints; mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact; @@ -196,8 +185,6 @@ void ContactSolver::initializeForIsland(Island* island) { 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[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact(); @@ -205,18 +192,10 @@ void ContactSolver::initializeForIsland(Island* island) { mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact; externalContact->setIsRestingContact(true); - //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) { - mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1; - mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2; - //} + frictionPointBody1 += p1; + frictionPointBody2 += p2; // Compute the velocity difference Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1); @@ -238,7 +217,7 @@ void ContactSolver::initializeForIsland(Island* island) { mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN; + mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = restitutionFactor * deltaVDotN; } // If the warm starting of the contact solver is active @@ -251,77 +230,70 @@ void ContactSolver::initializeForIsland(Island* island) { // Initialize the split impulses to zero mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0; - // If we solve the friction constraints at the center of the contact manifold - //if (mIsSolveFrictionAtContactManifoldCenterActive) { - mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal; - //} + mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal; mNbPenetrationConstraints++; nbContacts++; } - // If we solve the friction constraints at the center of the contact manifold - //if (mIsSolveFrictionAtContactManifoldCenterActive) { + frictionPointBody1 /= nbContacts; + frictionPointBody2 /= nbContacts; + mFrictionConstraints[mNbFrictionConstraints].r1Friction = frictionPointBody1 - x1; + mFrictionConstraints[mNbFrictionConstraints].r2Friction = frictionPointBody2 - x2; + mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1(); + mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2(); - //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(); + // If warm starting is active + if (mIsWarmStartingActive) { - // 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 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); + } - // 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(); - mFrictionConstraints[mNbFrictionConstraints].normal.normalize(); + Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - + v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); - Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) - - v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]); - // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]); - - // 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 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); mNbFrictionConstraints++; } @@ -463,9 +435,6 @@ void ContactSolver::solvePenetrationConstraints() { PROFILE("ContactSolver::solvePenetrationConstraints()"); - // TODO : Check that the PenetrationConstraint struct only contains variables that are - // used in this method, nothing more - // TODO : Maybe solve split impulses and normal impulses separately decimal deltaLambda; @@ -550,9 +519,6 @@ void ContactSolver::solvePenetrationConstraints() { // 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; isetFrictionVector1(mFrictionConstraints[i].frictionVector1); mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2); } - - /* - if (mPenetrationConstraints != nullptr) { - // TODO : Delete this - delete[] mPenetrationConstraints; - } - - if (mFrictionConstraints != nullptr) { - delete[] mFrictionConstraints; - } - */ } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index e5d5840d..0e710c6f 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -115,8 +115,6 @@ class ContactSolver { struct PenetrationConstraint { - // TODO : Pack bools into a single value - /// Index of body 1 in the constraint solver uint indexBody1; @@ -144,9 +142,6 @@ class ContactSolver { /// Velocity restitution bias decimal restitutionBias; - /// Mix of the restitution factor for two bodies - decimal restitutionFactor; - /// Accumulated normal impulse decimal penetrationImpulse; @@ -180,8 +175,6 @@ class ContactSolver { struct FrictionConstraint { - // TODO : Pack bools into a single value - /// Index of body 1 in the constraint solver uint indexBody1; @@ -194,12 +187,6 @@ class ContactSolver { /// 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; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 91312546..b8a48ddc 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -212,6 +212,9 @@ void DynamicsWorld::updateBodiesState() { // Update the transform of the body (using the new center of mass and new orientation) bodies[b]->updateTransformWithCenterOfMass(); + // Update the world inverse inertia tensor of the body + bodies[b]->updateInertiaTensorInverseWorld(); + // Update the broad-phase state of the body bodies[b]->updateBroadPhaseState(); } From 54be20c5d3bd9400aa71c1767faff8167fa9a808 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 2 Oct 2016 15:10:19 +0200 Subject: [PATCH 08/19] Increase the default size of the single frame memory allocator --- src/configuration.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/configuration.h b/src/configuration.h index 87747756..c14783d8 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -145,7 +145,7 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; /// Size (in bytes) of the single frame allocator -constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; +constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb } From 25fddd6fb2481f727ce1c1f1d688d8abaa635d16 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 8 Oct 2016 01:18:56 +0200 Subject: [PATCH 09/19] Back to previous contact solver --- src/engine/ContactSolver.cpp | 1161 ++++++++++++++++++++-------------- src/engine/ContactSolver.h | 319 +++++----- src/engine/DynamicsWorld.cpp | 57 +- 3 files changed, 878 insertions(+), 659 deletions(-) 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 From 3ab2b8608c99a9f40737f9104114628920d624a2 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 8 Oct 2016 16:58:28 +0200 Subject: [PATCH 10/19] Always solve friction at the center of the manifold and always use warmstarting --- src/engine/ContactSolver.cpp | 477 +++++++++++------------------------ src/engine/ContactSolver.h | 17 -- src/engine/DynamicsWorld.h | 14 - 3 files changed, 142 insertions(+), 366 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d8c3a644..77abee5d 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -45,8 +45,7 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity mContactConstraints(nullptr), mSingleFrameAllocator(allocator), mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mIsSplitImpulseActive(true), - mIsSolveFrictionAtContactManifoldCenterActive(true) { + mIsSplitImpulseActive(true) { } @@ -105,11 +104,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; internalManifold.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(); - } + internalManifold.frictionPointBody1 = Vector3::zero(); + internalManifold.frictionPointBody2 = Vector3::zero(); // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { @@ -137,40 +133,22 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { contactPoint.friction2Impulse = 0.0; contactPoint.rollingResistanceImpulse = Vector3::zero(); - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 += p1; - internalManifold.frictionPointBody2 += p2; - } + 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(); + 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); - } - } + // Initialize the accumulated impulses with the previous step accumulated impulses + internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); } // Fill-in all the matrices needed to solve the LCP problem @@ -190,9 +168,7 @@ void ContactSolver::initializeContactConstraints() { Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - manifold.normal = Vector3(0.0, 0.0, 0.0); - } + manifold.normal.setToZero(); // Get the velocities of the bodies const Vector3& v1 = mLinearVelocities[manifold.indexBody1]; @@ -220,37 +196,6 @@ void ContactSolver::initializeContactConstraints() { 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 @@ -261,23 +206,16 @@ void ContactSolver::initializeContactConstraints() { 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(); - } + // 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; - } + manifold.normal += contactPoint.normal; } // Compute the inverse K matrix for the rolling resistance constraint @@ -287,45 +225,41 @@ void ContactSolver::initializeContactConstraints() { manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); } - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { + manifold.normal.normalize(); - manifold.normal.normalize(); + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); - Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - - v1 - w1.cross(manifold.r1Friction); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, manifold); - // 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); - } + // 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); } } @@ -335,9 +269,6 @@ void ContactSolver::initializeContactConstraints() { /// the solution of the linear system void ContactSolver::warmStart() { - // Check that warm starting is active - if (!mIsWarmStartingActive) return; - // For each constraint for (uint c=0; c 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 @@ -420,7 +306,7 @@ void ContactSolver::warmStart() { // If we solve the friction constraints at the center of the contact manifold and there is // at least one resting contact point in the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { + if (atLeastOneRestingContactPoint) { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses @@ -588,189 +474,110 @@ void ContactSolver::solve() { 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 ------ // - // ------ 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 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 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); - // 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, + // 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(impulseFriction1, contactManifold); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); - // ------ Second friction constraint at the center of the contact manifol ----- // + // --------- Rolling resistance constraint at the center of the contact manifold --------- // + + if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + const Vector3 JvRolling = w2 - w1; // 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; + 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 - linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + angularImpulseBody1 = -deltaLambdaRolling; + angularImpulseBody2 = deltaLambdaRolling; + const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, + Vector3::zero(), 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); - } + applyImpulse(impulseRolling, contactManifold); } } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 7fd4442f..5af9433a 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -351,16 +351,9 @@ class ContactSolver { /// Reference to the map of rigid body to their index in the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; - /// True if the warm starting of the solver is active - bool mIsWarmStartingActive; - /// True if the split impulse position correction is active bool mIsSplitImpulseActive; - /// True if we solve 3 friction constraints at the contact manifold center only - /// instead of 2 friction constraints at each contact point - bool mIsSolveFrictionAtContactManifoldCenterActive; - // -------------------- Methods -------------------- // /// Initialize the contact constraints before solving the system @@ -446,10 +439,6 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Clean up the constraint solver void cleanup(); }; @@ -486,12 +475,6 @@ inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; } -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mIsSolveFrictionAtContactManifoldCenterActive = isActive; -} - // Compute the collision restitution factor from the restitution factor of each body inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) const { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index dec383a1..746e8255 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -204,10 +204,6 @@ class DynamicsWorld : public CollisionWorld { /// Set the position correction technique used for joints void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique); - /// Activate or deactivate the solving of friction constraints at the center of - /// the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Create a rigid body into the physics world. RigidBody* createRigidBody(const Transform& transform); @@ -367,16 +363,6 @@ inline void DynamicsWorld::setJointsPositionCorrectionTechnique( } } -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -/** - * @param isActive True if you want the friction to be solved at the center of - * the contact manifold and false otherwise - */ -inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); -} - // Return the gravity vector of the world /** * @return The current gravity vector (in meter per seconds squared) From a4a141483b31d46049a9165e9e9a93021b36a7ec Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 8 Oct 2016 23:04:22 +0200 Subject: [PATCH 11/19] Remove init contact constraint method --- src/engine/ContactSolver.cpp | 190 ++++++++++++++--------------------- src/engine/ContactSolver.h | 3 - 2 files changed, 76 insertions(+), 117 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 77abee5d..0a09f5d8 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -103,10 +103,16 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { internalManifold.externalContactManifold = externalManifold; internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; - + internalManifold.normal.setToZero(); internalManifold.frictionPointBody1 = Vector3::zero(); internalManifold.frictionPointBody2 = Vector3::zero(); + // Get the velocities of the bodies + const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1]; + const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1]; + const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2]; + const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2]; + // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { @@ -128,13 +134,38 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { 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(); + contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); + contactPoint.penetrationSplitImpulse = 0.0; + contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); + contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); + contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); internalManifold.frictionPointBody1 += p1; internalManifold.frictionPointBody2 += p2; + + // Compute the velocity difference + Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + + contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal); + contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); + + // Compute the inverse mass matrix K for the penetration constraint + decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + + ((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + + ((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); + contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); + + // 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 = internalManifold.restitutionFactor * deltaVDotN; + } + + internalManifold.normal += contactPoint.normal; } @@ -149,120 +180,51 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + + // Compute the inverse K matrix for the rolling resistance constraint + internalManifold.inverseRollingResistance.setToZero(); + if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) { + internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2; + internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse(); + } + + internalManifold.normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) - + v1 - w1.cross(internalManifold.r1Friction); + + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, internalManifold); + + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1); + internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2); + internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1); + internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2); + decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + + ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot( + internalManifold.frictionVector1) + + ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot( + internalManifold.frictionVector1); + decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + + ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot( + internalManifold.frictionVector2) + + ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot( + internalManifold.frictionVector2); + decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 * + internalManifold.normal) + + internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 * + internalManifold.normal); + internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); + internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); + internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); } // 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) / - massPenetration : - decimal(0.0); - - // Compute the restitution velocity bias "b". We compute this here instead - // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal - // velocity bellow a given threshold), we do not add a restitution velocity bias - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); - if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; - } - - // 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; - - 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(); - } - - 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. /// For each constraint, we apply the previous impulse (from the previous step) /// at the beginning. With this technique, we will converge faster towards @@ -682,7 +644,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { - assert(contact.normal.length() > 0.0); + assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 5af9433a..a465fa5a 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -356,9 +356,6 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// 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); From 7b5dce927e006d2ab17a8dff3d4f853bcffe1c4d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 10 Oct 2016 23:30:32 +0200 Subject: [PATCH 12/19] Fix issue with split impulse and refactor contact solver --- src/engine/ContactSolver.cpp | 196 +++++++++++++++-------------------- src/engine/ContactSolver.h | 49 +-------- 2 files changed, 82 insertions(+), 163 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 0a09f5d8..d02c773d 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -220,9 +220,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); } - - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); } // Warm start the solver. @@ -249,12 +246,14 @@ void ContactSolver::warmStart() { // --------- Penetration --------- // - // Compute the impulse P = J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse( - contactPoint.penetrationImpulse, contactPoint); + // Update the velocities of the body 1 by applying the impulse P + Vector3 impulsePenetration = contactPoint.normal * contactPoint.penetrationImpulse; + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-impulsePenetration); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * contactPoint.penetrationImpulse); - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); + // Update the velocities of the body 2 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * impulsePenetration; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * contactPoint.penetrationImpulse); } else { // If it is a new contact point @@ -272,72 +271,66 @@ void ContactSolver::warmStart() { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); + Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 + + contactManifold.friction2Impulse * contactManifold.oldFrictionVector2; + contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); + contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2); // ------ First friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifold ----- // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse; + linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse; + angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse; - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 2 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 2 by applying the impulse P + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Rolling resistance at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); + + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; } else { // If it is a new contact manifold @@ -402,12 +395,15 @@ void ContactSolver::solve() { deltaLambda, decimal(0.0)); deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - // Compute the impulse P=J^T * lambda - const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, - contactPoint); + Vector3 linearImpulse = contactPoint.normal * deltaLambda; - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda); + + // Update the velocities of the body 2 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda); sumPenetrationImpulse += contactPoint.penetrationImpulse; @@ -428,13 +424,19 @@ void ContactSolver::solve() { contactPoint.penetrationSplitImpulse = std::max( contactPoint.penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit; - // Compute the impulse P=J^T * lambda - const Impulse splitImpulsePenetration = computePenetrationImpulse( - deltaLambdaSplit, contactPoint); + Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit; - applySplitImpulse(splitImpulsePenetration, contactManifold); + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); + mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + (-contactPoint.r1CrossN * deltaLambdaSplit); + + // Update the velocities of the body 1 by applying the impulse P + mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; + mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + contactPoint.r2CrossN * deltaLambdaSplit; } } @@ -455,21 +457,22 @@ void ContactSolver::solve() { 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); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 2 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); + deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); Jv = deltaV.dot(contactManifold.frictionVector2); // Compute the Lagrange multiplier lambda @@ -482,15 +485,17 @@ void ContactSolver::solve() { 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); + // Update the velocities of the body 1 by applying the impulse P + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + + // Update the velocities of the body 2 by applying the impulse P + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // @@ -507,15 +512,13 @@ void ContactSolver::solve() { 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); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); + + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; // --------- Rolling resistance constraint at the center of the contact manifold --------- // @@ -532,14 +535,11 @@ void ContactSolver::solve() { 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); + // Update the velocities of the body 1 by applying the impulse P + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRolling, contactManifold); + // Update the velocities of the body 2 by applying the impulse P + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling; } } } @@ -575,40 +575,6 @@ void ContactSolver::storeImpulses() { } } -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - - // 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, diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index a465fa5a..802f7024 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -356,13 +356,6 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Apply an impulse to the two bodies of a constraint - 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); - /// Compute the collision restitution factor from the restitution factor of each body decimal computeMixedRestitutionFactor(RigidBody *body1, RigidBody *body2) const; @@ -386,18 +379,6 @@ class ContactSolver { void computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contactPoint) const; - /// Compute a penetration constraint impulse - const Impulse computePenetrationImpulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; - - /// Compute the first friction constraint impulse - const Impulse computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; - - /// Compute the second friction constraint impulse - const Impulse computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) const; - public: // -------------------- Methods -------------------- // @@ -486,7 +467,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()); } @@ -496,34 +477,6 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } -// 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); -} - -// Compute the first friction constraint impulse -inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, - -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, - contactPoint.r2CrossT1 * deltaLambda); -} - -// Compute the second friction constraint impulse -inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, - const ContactPointSolver& contactPoint) - const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, - -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, - contactPoint.r2CrossT2 * deltaLambda); -} - } #endif From 58ae61d6aaa0c72fad376a8d5fd2e16c10fc4d56 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 11 Oct 2016 20:08:47 +0200 Subject: [PATCH 13/19] Remove Impulse class --- CMakeLists.txt | 1 - src/engine/ContactSolver.h | 1 - src/engine/Impulse.h | 87 -------------------------------------- 3 files changed, 89 deletions(-) delete mode 100644 src/engine/Impulse.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 69f20522..87a02105 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,7 +147,6 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/DynamicsWorld.h" "src/engine/DynamicsWorld.cpp" "src/engine/EventListener.h" - "src/engine/Impulse.h" "src/engine/Island.h" "src/engine/Island.cpp" "src/engine/Material.h" diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 802f7024..02a3c390 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -32,7 +32,6 @@ #include "constraint/Joint.h" #include "collision/ContactManifold.h" #include "Island.h" -#include "Impulse.h" #include #include diff --git a/src/engine/Impulse.h b/src/engine/Impulse.h deleted file mode 100644 index 89e36429..00000000 --- a/src/engine/Impulse.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_IMPULSE_H -#define REACTPHYSICS3D_IMPULSE_H - -// Libraries -#include "mathematics/mathematics.h" - -namespace reactphysics3d { - -// Structure Impulse -/** - * Represents an impulse that we can apply to bodies in the contact or constraint solver. - */ -struct Impulse { - - private: - - // -------------------- Methods -------------------- // - - public: - - // -------------------- Attributes -------------------- // - - /// Linear impulse applied to the first body - const Vector3 linearImpulseBody1; - - /// Angular impulse applied to the first body - const Vector3 angularImpulseBody1; - - /// Linear impulse applied to the second body - const Vector3 linearImpulseBody2; - - /// Angular impulse applied to the second body - const Vector3 angularImpulseBody2; - - // -------------------- Methods -------------------- // - - /// Constructor - Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1, - const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2) - : linearImpulseBody1(initLinearImpulseBody1), - angularImpulseBody1(initAngularImpulseBody1), - linearImpulseBody2(initLinearImpulseBody2), - angularImpulseBody2(initAngularImpulseBody2) { - - } - - /// Copy-constructor - Impulse(const Impulse& impulse) - : linearImpulseBody1(impulse.linearImpulseBody1), - angularImpulseBody1(impulse.angularImpulseBody1), - linearImpulseBody2(impulse.linearImpulseBody2), - angularImpulseBody2(impulse.angularImpulseBody2) { - - } - - /// Deleted assignment operator - Impulse& operator=(const Impulse& impulse) = delete; -}; - -} - -#endif From d04cee7d0ae1e8b00da8d700bd11712211f2b17f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Oct 2016 15:40:38 +0200 Subject: [PATCH 14/19] Change the way to iterate over contacts --- src/engine/ContactSolver.cpp | 561 ++++++++++++++++++----------------- src/engine/ContactSolver.h | 25 +- src/engine/DynamicsWorld.cpp | 49 +-- 3 files changed, 339 insertions(+), 296 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d02c773d..b4e28aaa 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,7 +36,7 @@ using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP= decimal(0.01); +const decimal ContactSolver::SLOP = decimal(0.01); // Constructor ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, @@ -49,8 +49,54 @@ ContactSolver::ContactSolver(const std::map& mapBodyToVelocity } +// Initialize the contact constraints +void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { + + PROFILE("ContactSolver::init()"); + + mTimeStep = timeStep; + + // TODO : Try not to count manifolds and contact points here + uint nbContactManifolds = 0; + uint nbContactPoints = 0; + for (uint i = 0; i < nbIslands; i++) { + uint nbManifoldsInIsland = islands[i]->getNbContactManifolds(); + nbContactManifolds += nbManifoldsInIsland; + + for (uint j=0; j < nbManifoldsInIsland; j++) { + nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints(); + } + } + + mNbContactManifolds = 0; + mNbContactPoints = 0; + + mContactConstraints = nullptr; + mContactPoints = nullptr; + + if (nbContactManifolds == 0 || nbContactPoints == 0) return; + + // TODO : Count exactly the number of constraints to allocate here + mContactPoints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints)); + assert(mContactPoints != nullptr); + + mContactConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds)); + assert(mContactConstraints != nullptr); + + // For each island of the world + for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { + + if (islands[islandIndex]->getNbContactManifolds() > 0) { + initializeForIsland(islands[islandIndex]); + } + } + + // Warmstarting + warmStart(); +} + // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(Island* island) { PROFILE("ContactSolver::initializeForIsland()"); @@ -60,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); - // Set the current time step - mTimeStep = dt; - - mNbContactManifolds = island->getNbContactManifolds(); - - mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; - assert(mContactConstraints != nullptr); - // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifolds(); - for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& internalManifold = mContactConstraints[i]; - assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact @@ -90,34 +126,33 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { // Initialize the internal contact manifold structure using the external // contact manifold - internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.massInverseBody1 = body1->mMassInverse; - internalManifold.massInverseBody2 = body2->mMassInverse; - internalManifold.nbContacts = externalManifold->getNbContactPoints(); - internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; - internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; - internalManifold.normal.setToZero(); - internalManifold.frictionPointBody1 = Vector3::zero(); - internalManifold.frictionPointBody2 = Vector3::zero(); + new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); + mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; + mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse; + mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints(); + mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2); + mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); + mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold; + mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; + mContactConstraints[mNbContactManifolds].normal.setToZero(); + mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero(); + mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero(); // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1]; - const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1]; - const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2]; - const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2]; + const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; + const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; + const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; // For each contact point of the contact manifold for (uint c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalManifold.contacts[c]; - // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(c); @@ -125,100 +160,104 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.externalContact = externalContact; - contactPoint.normal = externalContact->getNormal(); - contactPoint.r1 = p1 - x1; - contactPoint.r2 = p2 - x2; - contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); - contactPoint.isRestingContact = externalContact->getIsRestingContact(); + new (mContactPoints + mNbContactPoints) ContactPointSolver(); + mContactPoints[mNbContactPoints].externalContact = externalContact; + mContactPoints[mNbContactPoints].normal = externalContact->getNormal(); + mContactPoints[mNbContactPoints].r1 = p1 - x1; + mContactPoints[mNbContactPoints].r2 = p2 - x2; + mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); + mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); - contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); - contactPoint.penetrationSplitImpulse = 0.0; - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); + mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1(); + mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2(); + mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); + mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; + mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1(); + mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2(); + mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); - internalManifold.frictionPointBody1 += p1; - internalManifold.frictionPointBody2 += p2; + mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; + mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; // Compute the velocity difference - Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); - contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal); - contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); + mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); // Compute the inverse mass matrix K for the penetration constraint - decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) + - ((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal); - contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); + decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal); + mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias - contactPoint.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contactPoint.normal); + mContactPoints[mNbContactPoints].restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - contactPoint.restitutionBias = internalManifold.restitutionFactor * deltaVDotN; + mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN; } - internalManifold.normal += contactPoint.normal; + mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; + + mNbContactPoints++; } - - internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); - internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); - internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; - internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; - internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); + mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast(mContactConstraints[mNbContactManifolds].nbContacts); + mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1; + mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2; + mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1(); + mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2(); // Initialize the accumulated impulses with the previous step accumulated impulses - internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); - internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); - internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); + mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1(); + mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2(); + mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); // Compute the inverse K matrix for the rolling resistance constraint - internalManifold.inverseRollingResistance.setToZero(); - if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) { - internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2; - internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse(); + mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); + if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) { + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; + mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); } - internalManifold.normal.normalize(); + mContactConstraints[mNbContactManifolds].normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) - - v1 - w1.cross(internalManifold.r1Friction); + Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) - + v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction); // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, internalManifold); + computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]); // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold - internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1); - internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2); - internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1); - internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2); - decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot( - internalManifold.frictionVector1) + - ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot( - internalManifold.frictionVector1); - decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 + - ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot( - internalManifold.frictionVector2) + - ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot( - internalManifold.frictionVector2); - decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 * - internalManifold.normal) + - internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 * - internalManifold.normal); - internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); - internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); - internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); + mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1); + mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2); + decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector1); + decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2) + + ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot( + mContactConstraints[mNbContactManifolds].frictionVector2); + decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * + mContactConstraints[mNbContactManifolds].normal) + + mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * + mContactConstraints[mNbContactManifolds].normal); + mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0); + mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0); + + mNbContactManifolds++; } } @@ -228,41 +267,41 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { /// the solution of the linear system void ContactSolver::warmStart() { + uint contactPointIndex = 0; + // For each constraint for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contactPoint.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contactPoint.restitutionBias) * - contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) * + mContactPoints[contactPointIndex].inversePenetrationMass; } else { - deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass; } - lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + + lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse; + mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse + deltaLambda, decimal(0.0)); - deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp; - Vector3 linearImpulse = contactPoint.normal * deltaLambda; + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda); + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda); // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda); + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda); - sumPenetrationImpulse += contactPoint.penetrationImpulse; + sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; // If the split impulse position correction is active if (mIsSplitImpulseActive) { // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - - v1Split - w1Split.cross(contactPoint.r1); - decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - + v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1); + decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal); decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * - contactPoint.inversePenetrationMass; - decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = std::max( - contactPoint.penetrationSplitImpulse + + mContactPoints[contactPointIndex].inversePenetrationMass; + decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse; + mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max( + mContactPoints[contactPointIndex].penetrationSplitImpulse + deltaLambdaSplit, decimal(0.0)); - deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit; - Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit; + Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse); - mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * - (-contactPoint.r1CrossN * deltaLambdaSplit); + mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); + mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * + (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse; - mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * - contactPoint.r2CrossN * deltaLambdaSplit; + mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; + mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * + mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit; } + + contactPointIndex++; } // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); - decimal Jv = deltaV.dot(contactManifold.frictionVector1); + Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) + - v1 - w1.cross(mContactConstraints[c].r1Friction); + decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1); // Compute the Lagrange multiplier lambda - decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; - decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction1Impulse + + decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass; + decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction1Impulse; + mContactConstraints[c].friction1Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); - Jv = deltaV.dot(contactManifold.frictionVector2); + deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction); + Jv = deltaV.dot(mContactConstraints[c].frictionVector2); // Compute the Lagrange multiplier lambda - deltaLambda = -Jv * contactManifold.inverseFriction2Mass; - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = std::max(-frictionLimit, - std::min(contactManifold.friction2Impulse + + deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass; + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].friction2Impulse; + mContactConstraints[c].friction2Impulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; - angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda; + linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda; + angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2); - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1; + mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2; - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifol ------ // // Compute J*v deltaV = w2 - w1; - Jv = deltaV.dot(contactManifold.normal); + Jv = deltaV.dot(mContactConstraints[c].normal); - deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); - frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = std::max(-frictionLimit, - std::min(contactManifold.frictionTwistImpulse + deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass); + frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = mContactConstraints[c].frictionTwistImpulse; + mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit, + std::min(mContactConstraints[c].frictionTwistImpulse + deltaLambda, frictionLimit)); - deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - angularImpulseBody2 = contactManifold.normal * deltaLambda; + angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2); // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; // --------- Rolling resistance constraint at the center of the contact manifold --------- // - if (contactManifold.rollingResistanceFactor > 0) { + if (mContactConstraints[c].rollingResistanceFactor > 0) { // Compute J*v const Vector3 JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); - decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; - Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; - contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + + Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling); + decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse; + Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse; + mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); - deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; + deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling); + mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling); // Update the velocities of the body 2 by applying the impulse P - mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling; + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; } } } @@ -548,30 +586,30 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { + uint contactPointIndex = 0; + // For each contact manifold for (uint c=0; csetPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse); + mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse); + mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse); + mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse); - ContactPointSolver& contactPoint = manifold.contacts[i]; + mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1); + mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2); - contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); + contactPointIndex++; } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse); + mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse); + mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse); + mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse); + mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1); + mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2); } } @@ -634,12 +672,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } - -// Clean up the constraint solver -void ContactSolver::cleanup() { - - if (mContactConstraints != nullptr) { - delete[] mContactConstraints; - mContactConstraints = nullptr; - } -} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 02a3c390..e4e2710e 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -220,11 +220,8 @@ class ContactSolver { /// Inverse inertia tensor of body 2 Matrix3x3 inverseInertiaTensorBody2; - /// Contact point constraints - ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - /// Number of contact points - uint nbContacts; + short int nbContacts; /// True if the body 1 is of type dynamic bool isBody1DynamicType; @@ -335,6 +332,12 @@ class ContactSolver { /// Contact constraints ContactManifoldSolver* mContactConstraints; + /// Contact points + ContactPointSolver* mContactPoints; + + /// Number of contact point constraints + uint mNbContactPoints; + /// Number of contact constraints uint mNbContactManifolds; @@ -378,6 +381,9 @@ class ContactSolver { void computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contactPoint) const; + /// Warm start the solver. + void warmStart(); + public: // -------------------- Methods -------------------- // @@ -389,8 +395,11 @@ class ContactSolver { /// Destructor ~ContactSolver() = default; + /// Initialize the contact constraints + void init(Island** islands, uint nbIslands, decimal timeStep); + /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + void initializeForIsland(Island* island); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -400,9 +409,6 @@ class ContactSolver { void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, Vector3* constrainedAngularVelocities); - /// Warm start the solver. - void warmStart(); - /// Store the computed impulses to use them to /// warm start the solver at the next iteration void storeImpulses(); @@ -415,9 +421,6 @@ class ContactSolver { /// Activate or Deactivate the split impulses for contacts void setIsSplitImpulseActive(bool isActive); - - /// Clean up the constraint solver - void cleanup(); }; // Set the split velocities arrays diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index c40d5ec5..d52348f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -342,23 +342,28 @@ void DynamicsWorld::solveContactsAndConstraints() { // ---------- Solve velocity constraints for joints and contacts ---------- // + // Initialize the contact solver + mContactSolver.init(mIslands, mNbIslands, mTimeStep); + // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + //if (!isConstraintsToSolve && !isContactsToSolve) continue; // If there are contacts in the current island - if (isContactsToSolve) { +// if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); +// // Initialize the solver +// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - mContactSolver.warmStart(); - } +// // Warm start the contact solver +// if (mContactSolver.IsWarmStartingActive()) { +// mContactSolver.warmStart(); +// } +// } // If there are constraints if (isConstraintsToSolve) { @@ -366,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() { // Initialize the constraint solver mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); } + } - // For each iteration of the velocity solver - for (uint i=0; igetNbJoints() > 0; if (isConstraintsToSolve) { mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); } - - // Solve the contacts - if (isContactsToSolve) mContactSolver.solve(); } - // Cache the lambda values in order to use them in the next - // step and cleanup the contact solver - if (isContactsToSolve) { - mContactSolver.storeImpulses(); - mContactSolver.cleanup(); - } + mContactSolver.solve(); + + // Solve the contacts +// if (isContactsToSolve) { + +// mContactSolver.resetTotalPenetrationImpulse(); + +// mContactSolver.solvePenetrationConstraints(); +// mContactSolver.solveFrictionConstraints(); +// } } + + mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints From 81426293e02d441f0bbefde7712955ceb8c93003 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Oct 2016 23:18:42 +0200 Subject: [PATCH 15/19] Remove unused variables in contact solver --- src/constraint/ContactPoint.cpp | 5 +- src/constraint/ContactPoint.h | 92 --------------------------------- src/engine/ContactSolver.cpp | 58 +++------------------ src/engine/ContactSolver.h | 42 ++------------- 4 files changed, 11 insertions(+), 186 deletions(-) diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 3be8476a..c0d567ea 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -45,9 +45,6 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) contactInfo.localPoint2), mIsRestingContact(false) { - mFrictionVectors[0] = Vector3(0, 0, 0); - mFrictionVectors[1] = Vector3(0, 0, 0); - - assert(mPenetrationDepth > 0.0); + assert(mPenetrationDepth > decimal(0.0)); } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index d2e8f189..18be17b0 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -128,21 +128,9 @@ class ContactPoint { /// True if the contact is a resting contact (exists for more than one time step) bool mIsRestingContact; - /// Two orthogonal vectors that span the tangential friction plane - Vector3 mFrictionVectors[2]; - /// Cached penetration impulse decimal mPenetrationImpulse; - /// Cached first friction impulse - decimal mFrictionImpulse1; - - /// Cached second friction impulse - decimal mFrictionImpulse2; - - /// Cached rolling resistance impulse - Vector3 mRollingResistanceImpulse; - public : // -------------------- Methods -------------------- // @@ -186,27 +174,9 @@ class ContactPoint { /// Return the cached penetration impulse decimal getPenetrationImpulse() const; - /// Return the cached first friction impulse - decimal getFrictionImpulse1() const; - - /// Return the cached second friction impulse - decimal getFrictionImpulse2() const; - - /// Return the cached rolling resistance impulse - Vector3 getRollingResistanceImpulse() const; - /// Set the cached penetration impulse void setPenetrationImpulse(decimal impulse); - /// Set the first cached friction impulse - void setFrictionImpulse1(decimal impulse); - - /// Set the second cached friction impulse - void setFrictionImpulse2(decimal impulse); - - /// Set the cached rolling resistance impulse - void setRollingResistanceImpulse(const Vector3& impulse); - /// Set the contact world point on body 1 void setWorldPointOnBody1(const Vector3& worldPoint); @@ -219,18 +189,6 @@ class ContactPoint { /// Set the mIsRestingContact variable void setIsRestingContact(bool isRestingContact); - /// Get the first friction vector - Vector3 getFrictionVector1() const; - - /// Set the first friction vector - void setFrictionVector1(const Vector3& frictionVector1); - - /// Get the second friction vector - Vector3 getFrictionVector2() const; - - /// Set the second friction vector - void setFrictionVector2(const Vector3& frictionVector2); - /// Return the penetration depth decimal getPenetrationDepth() const; @@ -283,41 +241,11 @@ inline decimal ContactPoint::getPenetrationImpulse() const { return mPenetrationImpulse; } -// Return the cached first friction impulse -inline decimal ContactPoint::getFrictionImpulse1() const { - return mFrictionImpulse1; -} - -// Return the cached second friction impulse -inline decimal ContactPoint::getFrictionImpulse2() const { - return mFrictionImpulse2; -} - -// Return the cached rolling resistance impulse -inline Vector3 ContactPoint::getRollingResistanceImpulse() const { - return mRollingResistanceImpulse; -} - // Set the cached penetration impulse inline void ContactPoint::setPenetrationImpulse(decimal impulse) { mPenetrationImpulse = impulse; } -// Set the first cached friction impulse -inline void ContactPoint::setFrictionImpulse1(decimal impulse) { - mFrictionImpulse1 = impulse; -} - -// Set the second cached friction impulse -inline void ContactPoint::setFrictionImpulse2(decimal impulse) { - mFrictionImpulse2 = impulse; -} - -// Set the cached rolling resistance impulse -inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) { - mRollingResistanceImpulse = impulse; -} - // Set the contact world point on body 1 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; @@ -338,26 +266,6 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } -// Get the first friction vector -inline Vector3 ContactPoint::getFrictionVector1() const { - return mFrictionVectors[0]; -} - -// Set the first friction vector -inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { - mFrictionVectors[0] = frictionVector1; -} - -// Get the second friction vector -inline Vector3 ContactPoint::getFrictionVector2() const { - return mFrictionVectors[1]; -} - -// Set the second friction vector -inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { - mFrictionVectors[1] = frictionVector2; -} - // Return the penetration depth of the contact inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index b4e28aaa..34e86472 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -134,15 +134,12 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints(); - mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2); mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold; - mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; - mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].normal.setToZero(); - mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero(); - mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero(); + mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero(); + mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero(); // Get the velocities of the bodies const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; @@ -168,13 +165,8 @@ void ContactSolver::initializeForIsland(Island* island) { mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth(); mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact(); externalContact->setIsRestingContact(true); - mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1(); - mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2(); mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse(); mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0; - mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1(); - mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2(); - mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1; mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2; @@ -197,8 +189,9 @@ void ContactSolver::initializeForIsland(Island* island) { // velocity bellow a given threshold), we do not add a restitution velocity bias mContactPoints[mNbContactPoints].restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal); + const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { - mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN; + mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN; } mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal; @@ -219,8 +212,10 @@ void ContactSolver::initializeForIsland(Island* island) { mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); // Compute the inverse K matrix for the rolling resistance constraint + bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; + bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero(); - if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) { + if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) { mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2; mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse(); } @@ -296,9 +291,6 @@ void ContactSolver::warmStart() { // Initialize the accumulated impulses to zero mContactPoints[contactPointIndex].penetrationImpulse = 0.0; - mContactPoints[contactPointIndex].friction1Impulse = 0.0; - mContactPoints[contactPointIndex].friction2Impulse = 0.0; - mContactPoints[contactPointIndex].rollingResistanceImpulse = Vector3::zero(); } contactPointIndex++; @@ -594,12 +586,6 @@ void ContactSolver::storeImpulses() { for (short int i=0; isetPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse); - mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse); - mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse); - mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse); - - mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1); - mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2); contactPointIndex++; } @@ -613,36 +599,6 @@ void ContactSolver::storeImpulses() { } } -// 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(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) { - - // 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(); - } - - // 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, diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index e4e2710e..becd5529 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -122,33 +122,12 @@ class ContactSolver { /// Accumulated normal impulse decimal penetrationImpulse; - /// Accumulated impulse in the 1st friction direction - decimal friction1Impulse; - - /// Accumulated impulse in the 2nd friction direction - decimal friction2Impulse; - /// Accumulated split impulse for penetration correction decimal penetrationSplitImpulse; - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; - /// Normal vector of the contact Vector3 normal; - /// First friction vector in the tangent plane - Vector3 frictionVector1; - - /// Second friction vector in the tangent plane - Vector3 frictionVector2; - - /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; - - /// Old second friction vector in the tangent plane - Vector3 oldFrictionVector2; - /// Vector from the body 1 center to the contact point Vector3 r1; @@ -188,11 +167,11 @@ class ContactSolver { /// Inverse of the matrix K for the 2nd friction decimal inverseFriction2Mass; - /// True if the contact was existing last time step - bool isRestingContact; - /// Pointer to the external contact ContactPoint* externalContact; + + /// True if the contact was existing last time step + bool isRestingContact; }; // Structure ContactManifoldSolver @@ -223,15 +202,6 @@ class ContactSolver { /// Number of contact points short int 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; @@ -369,12 +339,6 @@ class ContactSolver { /// Compute th mixed rolling resistance factor between two bodies decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const; - /// 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; - /// 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. From 14bfb0aca4160d8323760ccb8d77f0bbc37fc0a3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 17 Oct 2016 22:41:33 +0200 Subject: [PATCH 16/19] Some optimizations in contact solver --- src/collision/ContactManifold.h | 6 ++-- src/engine/ContactSolver.h | 58 ++++++++++++--------------------- 2 files changed, 23 insertions(+), 41 deletions(-) diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 2f77e9b2..c36c1476 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -102,7 +102,7 @@ class ContactManifold { short int mNormalDirectionId; /// Number of contacts in the cache - uint mNbContactPoints; + int8 mNbContactPoints; /// First friction vector of the contact manifold Vector3 mFrictionVector1; @@ -187,7 +187,7 @@ class ContactManifold { void clear(); /// Return the number of contact points in the manifold - uint getNbContactPoints() const; + int8 getNbContactPoints() const; /// Return the first friction vector at the center of the contact manifold const Vector3& getFrictionVector1() const; @@ -264,7 +264,7 @@ inline short int ContactManifold::getNormalDirectionId() const { } // Return the number of contact points in the manifold -inline uint ContactManifold::getNbContactPoints() const { +inline int8 ContactManifold::getNbContactPoints() const { return mNbContactPoints; } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index becd5529..089d8b53 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -119,11 +119,8 @@ class ContactSolver { */ struct ContactPointSolver { - /// Accumulated normal impulse - decimal penetrationImpulse; - - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; + /// Pointer to the external contact + ContactPoint* externalContact; /// Normal vector of the contact Vector3 normal; @@ -134,41 +131,26 @@ class ContactSolver { /// Vector from the body 2 center to the contact point Vector3 r2; - /// 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; - - /// 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; + /// Accumulated normal impulse + decimal penetrationImpulse; + + /// Accumulated split impulse for penetration correction + decimal penetrationSplitImpulse; + /// Inverse of the matrix K for the penenetration decimal inversePenetrationMass; - /// Inverse of the matrix K for the 1st friction - decimal inverseFriction1Mass; + /// Cross product of r1 with the contact normal + Vector3 r1CrossN; - /// Inverse of the matrix K for the 2nd friction - decimal inverseFriction2Mass; - - /// Pointer to the external contact - ContactPoint* externalContact; + /// Cross product of r2 with the contact normal + Vector3 r2CrossN; /// True if the contact was existing last time step bool isRestingContact; @@ -181,11 +163,14 @@ class ContactSolver { */ struct ContactManifoldSolver { + /// Pointer to the external contact manifold + ContactManifold* externalContactManifold; + /// Index of body 1 in the constraint solver - uint indexBody1; + int32 indexBody1; /// Index of body 2 in the constraint solver - uint indexBody2; + int32 indexBody2; /// Inverse of the mass of body 1 decimal massInverseBody1; @@ -199,18 +184,12 @@ class ContactSolver { /// Inverse inertia tensor of body 2 Matrix3x3 inverseInertiaTensorBody2; - /// Number of contact points - short int nbContacts; - /// 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 @@ -275,6 +254,9 @@ class ContactSolver { /// Rolling resistance impulse Vector3 rollingResistanceImpulse; + + /// Number of contact points + int8 nbContacts; }; // -------------------- Constants --------------------- // From ce06a4b935e15c7fa4376c549572c8153cfcf02e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 17 Oct 2016 22:41:58 +0200 Subject: [PATCH 17/19] Change fixed size data types --- src/configuration.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/configuration.h b/src/configuration.h index c14783d8..04cb0956 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -30,6 +30,7 @@ #include #include #include +#include #include "decimal.h" // Windows platform @@ -51,10 +52,9 @@ using luint = long unsigned int; using bodyindex = luint; using bodyindexpair = std::pair; -using int16 = signed short; -using int32 = signed int; -using uint16 = unsigned short; -using uint32 = unsigned int; +using int8 = int8_t; +using int16 = int16_t; +using int32 = int32_t; // ------------------- Enumerations ------------------- // From cc6d3d621d4ef3b976ba091ce953da49f5bd464d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 20 Oct 2016 19:16:55 +0200 Subject: [PATCH 18/19] Add profiling data --- src/engine/ContactSolver.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 34e86472..837fdf38 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -262,6 +262,8 @@ void ContactSolver::initializeForIsland(Island* island) { /// the solution of the linear system void ContactSolver::warmStart() { + PROFILE("ContactSolver::warmStart()"); + uint contactPointIndex = 0; // For each constraint @@ -578,6 +580,8 @@ void ContactSolver::solve() { // warm start the solver at the next iteration void ContactSolver::storeImpulses() { + PROFILE("ContactSolver::storeImpulses()"); + uint contactPointIndex = 0; // For each contact manifold @@ -604,6 +608,8 @@ void ContactSolver::storeImpulses() { void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { + PROFILE("ContactSolver::computeFrictionVectors()"); + assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane From b3d24e4299a6503bcf1635475ae2da7694bfcb42 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 23 Oct 2016 20:04:52 +0200 Subject: [PATCH 19/19] Cache some calculation in contact solver --- src/engine/ContactSolver.cpp | 45 ++++++++++++++++++------------------ src/engine/ContactSolver.h | 4 ++-- src/mathematics/Vector3.h | 2 +- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 837fdf38..5d61243a 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -174,13 +174,16 @@ void ContactSolver::initializeForIsland(Island* island) { // Compute the velocity difference Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1); - mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); - mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal); + Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal); + + mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN; + mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN; // Compute the inverse mass matrix K for the penetration constraint decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 + - ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) + - ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal); + ((mContactPoints[mNbContactPoints].i1TimesR1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) + + ((mContactPoints[mNbContactPoints].i2TimesR2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal); mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0); // Compute the restitution velocity bias "b". We compute this here instead @@ -282,12 +285,12 @@ void ContactSolver::warmStart() { // Update the velocities of the body 1 by applying the impulse P Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse; - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-impulsePenetration); - mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * mContactPoints[contactPointIndex].penetrationImpulse); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration; + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * mContactPoints[contactPointIndex].penetrationImpulse); + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -320,7 +323,7 @@ void ContactSolver::warmStart() { mContactConstraints[c].friction1Impulse; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P @@ -335,7 +338,7 @@ void ContactSolver::warmStart() { angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P @@ -360,7 +363,7 @@ void ContactSolver::warmStart() { angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; @@ -428,12 +431,12 @@ void ContactSolver::solve() { Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); - mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda; // Update the velocities of the body 2 by applying the impulse P mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda); + mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -459,14 +462,12 @@ void ContactSolver::solve() { Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse); - mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * - (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit); + mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse; + mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse; - mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * - mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit; + mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit; } contactPointIndex++; @@ -494,7 +495,7 @@ void ContactSolver::solve() { Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P @@ -522,7 +523,7 @@ void ContactSolver::solve() { angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2); + mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P @@ -547,7 +548,7 @@ void ContactSolver::solve() { angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2); + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; @@ -568,7 +569,7 @@ void ContactSolver::solve() { deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling; // Update the velocities of the body 1 by applying the impulse P - mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling); + mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling; // Update the velocities of the body 2 by applying the impulse P mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling; diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 089d8b53..40e0721d 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -147,10 +147,10 @@ class ContactSolver { decimal inversePenetrationMass; /// Cross product of r1 with the contact normal - Vector3 r1CrossN; + Vector3 i1TimesR1CrossN; /// Cross product of r2 with the contact normal - Vector3 r2CrossN; + Vector3 i2TimesR2CrossN; /// True if the contact was existing last time step bool isRestingContact; diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 4f30391d..1ca1adfd 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -184,7 +184,7 @@ inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) { // Return the length of the vector inline decimal Vector3::length() const { - return sqrt(x*x + y*y + z*z); + return std::sqrt(x*x + y*y + z*z); } // Return the square of the length of the vector