From a4a141483b31d46049a9165e9e9a93021b36a7ec Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 8 Oct 2016 23:04:22 +0200 Subject: [PATCH] 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);