diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 6bd8a941..8e81b266 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,11 +31,16 @@ using namespace reactphysics3d; using namespace std; +// Constants initialization +const decimal ConstraintSolver::BETA = 0.2; +const decimal ConstraintSolver::BETA_SPLIT_IMPULSE = 0.2; +const decimal ConstraintSolver::SLOP = 0.01; + // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), - mIsSplitImpulseActive(true) { + mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -49,7 +54,7 @@ void ConstraintSolver::initialize() { nbConstraints = 0; - // TOOD : Use better allocation here + // TODO : Use better allocation here mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; mNbContactConstraints = 0; @@ -87,6 +92,13 @@ void ConstraintSolver::initialize() { constraint.massInverseBody2 = body2->getMassInverse(); constraint.nbContacts = contactManifold.nbContacts; constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2); + constraint.contactManifold = &contactManifold; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + constraint.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + } // For each contact point of the contact manifold for (uint c=0; csetIsRestingContact(true); contactPointConstraint.oldFrictionVector1 = contact->getFrictionVector1(); contactPointConstraint.oldFrictionVector2 = contact->getFrictionVector2(); + contactPointConstraint.penetrationImpulse = 0.0; + contactPointConstraint.friction1Impulse = 0.0; + contactPointConstraint.friction2Impulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 += p1; + constraint.frictionPointBody2 += p2; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 /= constraint.nbContacts; + constraint.frictionPointBody2 /= constraint.nbContacts; + constraint.r1Friction = constraint.frictionPointBody1 - x1; + constraint.r2Friction = constraint.frictionPointBody2 - x2; + constraint.oldFrictionVector1 = contactManifold.frictionVector1; + constraint.oldFrictionVector2 = contactManifold.frictionVector2; + + if (mIsWarmStartingActive) { + constraint.friction1Impulse = contactManifold.friction1Impulse; + constraint.friction2Impulse = contactManifold.friction2Impulse; + constraint.frictionTwistImpulse = contactManifold.frictionTwistImpulse; + } + else { + constraint.friction1Impulse = 0.0; + constraint.friction2Impulse = 0.0; + constraint.frictionTwistImpulse = 0.0; + } } mNbContactConstraints++; @@ -154,27 +196,26 @@ void ConstraintSolver::initializeContactConstraints() { Matrix3x3& I1 = constraint.inverseInertiaTensorBody1; Matrix3x3& I2 = constraint.inverseInertiaTensorBody2; + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.normal = Vector3(0.0, 0.0, 0.0); + } + + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + // For each contact point constraint for (uint i=0; i 0.0 ? contact.inversePenetrationMass = 1.0 / massPenetration : 0.0; - // Compute the inverse mass matrix K for the friction constraints - decimal friction1Mass = 0.0; - decimal friction2Mass = 0.0; - if (constraint.isBody1Moving) { - friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // Compute the friction vectors + computeFrictionVectors(deltaV, contact); + + contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1); + contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2); + contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1); + contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2); + + // Compute the inverse mass matrix K for the friction constraints at each contact point + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (constraint.isBody1Moving) { + friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + } + if (constraint.isBody2Moving) { + friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); + } + friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; + friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; } - if (constraint.isBody2Moving) { - friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); - } - friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; - friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 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 @@ -213,12 +265,50 @@ void ConstraintSolver::initializeContactConstraints() { } // Get the cached lambda values of the constraint - contact.penetrationImpulse = realContact->getCachedLambda(0); - contact.friction1Impulse = realContact->getCachedLambda(1); - contact.friction2Impulse = realContact->getCachedLambda(2); + if (mIsWarmStartingActive) { + contact.penetrationImpulse = realContact->getCachedLambda(0); + contact.friction1Impulse = realContact->getCachedLambda(1); + contact.friction2Impulse = realContact->getCachedLambda(2); + } // Initialize the split impulses to zero contact.penetrationSplitImpulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.normal += contact.normal; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + constraint.normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(constraint.r2Friction) - + v1 - w1.cross(constraint.r1Friction); + + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, constraint); + + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + constraint.r1CrossT1 = constraint.r1Friction.cross(constraint.frictionVector1); + constraint.r1CrossT2 = constraint.r1Friction.cross(constraint.frictionVector2); + constraint.r2CrossT1 = constraint.r2Friction.cross(constraint.frictionVector1); + constraint.r2CrossT2 = constraint.r2Friction.cross(constraint.frictionVector2); + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (constraint.isBody1Moving) { + friction1Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT1).cross(constraint.r1Friction)).dot(constraint.frictionVector1); + friction2Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT2).cross(constraint.r1Friction)).dot(constraint.frictionVector2); + } + if (constraint.isBody2Moving) { + friction1Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT1).cross(constraint.r2Friction)).dot(constraint.frictionVector1); + friction2Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT2).cross(constraint.r2Friction)).dot(constraint.frictionVector2); + } + friction1Mass > 0.0 ? constraint.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; + friction2Mass > 0.0 ? constraint.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; } } } @@ -234,6 +324,8 @@ void ConstraintSolver::warmStart() { ContactConstraint& constraint = mContactConstraints[c]; + bool atLeastOneRestingContactPoint = false; + for (uint i=0; i slop) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contact.penetrationDepth - slop)); + if (contact.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contact.penetrationDepth - SLOP)); decimal b = biasPenetrationDepth + contact.restitutionBias; // Compute the Lagrange multiplier @@ -358,6 +517,8 @@ void ConstraintSolver::solveContactConstraints() { // Apply the impulse to the bodies of the constraint applyImpulse(impulsePenetration, constraint); + sumPenetrationImpulse += contact.penetrationImpulse; + // If the split impulse position correction is active if (mIsSplitImpulseActive) { @@ -384,53 +545,136 @@ void ConstraintSolver::solveContactConstraints() { applySplitImpulse(splitImpulsePenetration, constraint); } - // --------- Friction 1 --------- // + // 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(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector1); + + deltaLambda = -Jv; + deltaLambda *= contact.inverseFriction1Mass; + decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here + lambdaTemp = contact.friction1Impulse; + contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = contact.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.frictionVector1 * deltaLambda; + angularImpulseBody1 = -contact.r1CrossT1 * deltaLambda; + linearImpulseBody2 = contact.frictionVector1 * deltaLambda; + angularImpulseBody2 = contact.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, constraint); + + // --------- Friction 2 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector2); + + deltaLambda = -Jv; + deltaLambda *= contact.inverseFriction2Mass; + frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here + lambdaTemp = contact.friction2Impulse; + contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = contact.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contact.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contact.frictionVector2 * deltaLambda; + angularImpulseBody2 = contact.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, constraint); + } + } + + // 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 - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector1); + Vector3 deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); + decimal Jv = deltaV.dot(constraint.frictionVector1); - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction1Mass; - decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction1Impulse; - contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction1Impulse - lambdaTemp; + decimal deltaLambda = -Jv * constraint.inverseFriction1Mass; + decimal frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.friction1Impulse; + constraint.friction1Impulse = std::max(-frictionLimit, std::min(constraint.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector1 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT1 * deltaLambda; - linearImpulseBody2 = contact.frictionVector1 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT1 * deltaLambda; + Vector3 linearImpulseBody1 = -constraint.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -constraint.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = constraint.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = constraint.r2CrossT1 * deltaLambda; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, constraint); - // --------- Friction 2 --------- // + // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector2); + deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); + Jv = deltaV.dot(constraint.frictionVector2); - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction2Mass; - frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction2Impulse; - contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction2Impulse - lambdaTemp; + deltaLambda = -Jv * constraint.inverseFriction2Mass; + frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.friction2Impulse; + constraint.friction2Impulse = std::max(-frictionLimit, std::min(constraint.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contact.frictionVector2 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT2 * deltaLambda; + linearImpulseBody1 = -constraint.frictionVector2 * deltaLambda; + angularImpulseBody1 = -constraint.r1CrossT2 * deltaLambda; + linearImpulseBody2 = constraint.frictionVector2 * deltaLambda; + angularImpulseBody2 = constraint.r2CrossT2 * deltaLambda; const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, constraint); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // TODO : Put this in the initialization method + decimal K = constraint.normal.dot(constraint.inverseInertiaTensorBody1 * constraint.normal) + + constraint.normal.dot(constraint.inverseInertiaTensorBody2 * constraint.normal); + + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(constraint.normal); + + // TODO : Compute the inverse mass matrix here for twist friction + deltaLambda = -Jv * (1.0 / K); + frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.frictionTwistImpulse; + constraint.frictionTwistImpulse = std::max(-frictionLimit, std::min(constraint.frictionTwistImpulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -constraint.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = constraint.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, constraint); } } } @@ -481,6 +725,12 @@ void ConstraintSolver::storeImpulses() { contact.contact->setFrictionVector1(contact.frictionVector1); contact.contact->setFrictionVector2(contact.frictionVector2); } + + constraint.contactManifold->friction1Impulse = constraint.friction1Impulse; + constraint.contactManifold->friction2Impulse = constraint.friction2Impulse; + constraint.contactManifold->frictionTwistImpulse = constraint.frictionTwistImpulse; + constraint.contactManifold->frictionVector1 = constraint.frictionVector1; + constraint.contactManifold->frictionVector2 = constraint.frictionVector2; } } @@ -521,7 +771,7 @@ void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactCo } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// The two vectors have to be such that : t1 x t2 = contactNormal +// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal. void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactPointConstraint& contact) const { @@ -553,3 +803,33 @@ void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactConstraint& 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 > 0.0) { + + // 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(); +} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 9675a63e..0125dd5e 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -28,6 +28,7 @@ // Libraries #include "../constraint/Contact.h" +#include "ContactManifold.h" #include "../configuration.h" #include "../constraint/Constraint.h" #include @@ -92,42 +93,109 @@ struct ContactConstraint { // TODO : Use a constant for the number of contact points - uint indexBody1; // Index of body 1 in the constraint solver - uint indexBody2; // Index of body 2 in the constraint solver - decimal massInverseBody1; // Inverse of the mass of body 1 - decimal massInverseBody2; // Inverse of the mass of body 2 - Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 - bool isBody1Moving; // True if the body 1 is allowed to move - bool isBody2Moving; // True if the body 2 is allowed to move - ContactPointConstraint contacts[4]; // Contact point constraints - uint nbContacts; // Number of contact points - decimal restitutionFactor; // Mix of the restitution factor for two bodies + uint indexBody1; // Index of body 1 in the constraint solver + uint indexBody2; // Index of body 2 in the constraint solver + decimal massInverseBody1; // Inverse of the mass of body 1 + decimal massInverseBody2; // Inverse of the mass of body 2 + Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 + bool isBody1Moving; // True if the body 1 is allowed to move + bool isBody2Moving; // True if the body 2 is allowed to move + ContactPointConstraint contacts[4]; // Contact point constraints + uint nbContacts; // Number of contact points + decimal restitutionFactor; // Mix of the restitution factor for two bodies + ContactManifold* contactManifold; // Contact manifold + + // --- Variables used when friction constraints are apply at the center of the manifold --- // + + Vector3 normal; // Average normal vector of the contact manifold + Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints + Vector3 r1Friction; // R1 vector for the friction constraints + Vector3 r2Friction; // R2 vector for the friction constraints + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + decimal inverseFriction1Mass; // Matrix K for the first friction constraint + decimal inverseFriction2Mass; // Matrix K for the second friction constraint + Vector3 frictionVector1; // First friction direction at contact manifold center + Vector3 frictionVector2; // Second friction direction at contact manifold center + Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center + decimal friction1Impulse; // First friction direction impulse at manifold center + decimal friction2Impulse; // Second friction direction impulse at manifold center + decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center }; -// TODO : Rewrite this coment to use the Sequential Impulse technique /* ------------------------------------------------------------------- Class ConstrainSolver : - This class represents the constraint solver. The constraint solver - is based on the theory from the paper "Iterative Dynamics with - Temporal Coherence" from Erin Catto. We keep the same notations as - in the paper. The idea is to construct a LCP problem and then solve - it using a Projected Gauss Seidel (PGS) solver. - - The idea is to solve the following system for lambda : - JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0 - - By default, error correction using first world order projections (described - by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and - Versatile Multibody Dynamics Animation") is used for very large penetration - depths. Error correction with projection requires to solve another LCP problem - that is simpler than the above one and by considering only the contact - constraints. The LCP problem for error correction is the following one : - J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0 - - where "d" is a vector with penetration depths. If the penetration depth of - a given contact is not very large, we use the Baumgarte error correction (see - the paper from Erin Catto). + This class represents the constraint solver that is used is solve constraints and + rigid bodies contacts. The constraint solver is based on the "Sequential Impulse" technique + described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + + A constraint between two bodies is represented by a function C(x) which is equal to zero + when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + bodies and b is the constraint bias. We are looking for a force F_c that will act on the + bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + multiplier lambda. + + An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + body to change its velocity. The idea of the Sequential Impulse technique is to apply + impulses to bodies of each constraints in order to keep the constraint satisfied. + + --- Step 1 --- + + First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + we obtain some new velocities v2' that tends to violate the constraints. + + v2' = v1 + dt * M^-1 * F_a + + where M is a matrix that contains mass and inertia tensor information. + + --- Step 2 --- + + During the second step, we iterate over all the constraints for a certain number of + iterations and for each constraint we compute the impulse to apply to the bodies needed + so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + P_c is the constraint impulse to apply to the body. Therefore, we have + v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + the bodies to satisfy the constraint. + + --- Step 3 --- + + In the third step, we integrate the new position x2 of the bodies using the new velocities + v2 computed in the second step with : x2 = x1 + dt * v2. + + Note that in the following code (as it is also explained in the slides from Erin Catto), + the value lambda is not only the lagrange multiplier but is the multiplication of the + Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + lambda, we mean (lambda * dt). + + We are using the accumulated impulse technique that is also described in the slides from + Erin Catto. + + We are also using warm starting. The idea is to warm start the solver at the beginning of + each step by applying the last impulstes for the constraints that we already existing at the + previous step. This allows the iterative solver to converge faster towards the solution. + + For contact constraints, we are also using split impulses so that the position correction + that uses Baumgarte stabilization does not change the momentum of the bodies. + + There are two ways to apply the friction constraints. Either the friction constraints are + applied at each contact point or they are applied only at the center of the contact manifold + between two bodies. If we solve the friction constraints at each contact point, we need + two constraints (two tangential friction directions) and if we solve the friction constraints + at the center of the contact manifold, we need two constraints for tangential friction but + also another twist friction constraint to prevent spin of the body around the contact + manifold center. ------------------------------------------------------------------- */ @@ -135,6 +203,17 @@ class ConstraintSolver { private: + // -------------------- Constants --------------------- // + + // Beta value for the penetration depth position correction without split impulses + static const decimal BETA; + + // Beta value for the penetration depth position correction with split impulses + static const decimal BETA_SPLIT_IMPULSE; + + // Slop distance (allowed penetration distance between bodies) + static const decimal SLOP; + // -------------------- Attributes -------------------- // DynamicsWorld* world; // Reference to the world @@ -174,6 +253,10 @@ class ConstraintSolver { // 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 constraint solver @@ -204,11 +287,17 @@ class ConstraintSolver { // Compute the collision restitution factor from the restitution factor of each body decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane - // The two vectors have to be such that : t1 x t2 = contactNormal + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact point constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. void computeFrictionVectors(const Vector3& deltaVelocity, ContactPointConstraint& contact) const; + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, ContactConstraint& contact) const; + public: // -------------------- Methods -------------------- // @@ -240,8 +329,15 @@ class ConstraintSolver { // Clean up the constraint solver void cleanup(); - // Set the number of iterations of the LCP solver - void setNbLCPIterations(uint mNbIterations); + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // 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); }; // Return true if the body is in at least one constraint @@ -297,11 +393,22 @@ inline void ConstraintSolver::cleanup() { } } -// Set the number of iterations of the LCP solver -inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { +// Set the number of iterations of the constraint solver +inline void ConstraintSolver::setNbIterationsSolver(uint nbIterations) { mNbIterations = nbIterations; } +// Activate or Deactivate the split impulses for contacts +inline void ConstraintSolver::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 ConstraintSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mIsSolveFrictionAtContactManifoldCenterActive = isActive; +} + // Compute the collision restitution factor from the restitution factor of each body inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1, const RigidBody* body2) const { diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h index ce5a7c67..7e8a7816 100644 --- a/src/engine/ContactManifold.h +++ b/src/engine/ContactManifold.h @@ -44,10 +44,26 @@ struct ContactManifold { // Number of contacts in the manifold uint nbContacts; + // First friction vector of the contact manifold + Vector3 frictionVector1; + + // Second friction vector of the contact manifold + Vector3 frictionVector2; + + // First friction constraint accumulated impulse + decimal friction1Impulse; + + // Second friction constraint accumulated impulse + decimal friction2Impulse; + + // Twist friction constraint accumulated impulse + decimal frictionTwistImpulse; + // -------------------- Methods -------------------- // // Constructor - ContactManifold() : nbContacts(0) { } + ContactManifold() : nbContacts(0), friction1Impulse(0.0), friction2Impulse(0.0), + frictionTwistImpulse(0.0) {} }; } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 2d970784..ae8b685a 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -139,8 +139,15 @@ public : // Update the physics simulation void update(); - // Set the number of iterations of the LCP solver - void setNbLCPIterations(uint nbIterations); + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // 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); // Set the isErrorCorrectionActive value void setIsErrorCorrectionActive(bool isErrorCorrectionActive); @@ -205,10 +212,21 @@ inline void DynamicsWorld::stop() { mTimer.stop(); } -// Set the number of iterations of the LCP solver -inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { - mConstraintSolver.setNbLCPIterations(nbIterations); -} +// Set the number of iterations of the constraint solver +inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { + mConstraintSolver.setNbIterationsSolver(nbIterations); +} + +// Activate or Deactivate the split impulses for contacts +inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) { + mConstraintSolver.setIsSplitImpulseActive(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 DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mConstraintSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); +} // Reset the boolean movement variable of each body inline void DynamicsWorld::resetBodiesMovementVariable() { diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 188c9933..c44e3820 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -26,7 +26,6 @@ // Libraries #include "Vector3.h" #include -#include #include // Namespaces diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index f8599c4e..bc549a08 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include "mathematics_functions.h" #include "../decimal.h" @@ -113,6 +114,9 @@ class Vector3 { // Cross product of two vectors Vector3 cross(const Vector3& vector) const; + // Normalize the vector + void normalize(); + // Return the corresponding absolute value vector Vector3 getAbsoluteVector() const; @@ -140,6 +144,9 @@ class Vector3 { // Overloaded operator for multiplication with a number with assignment Vector3& operator*=(decimal number); + // Overloaded operator for division by a number with assignment + Vector3& operator/=(decimal number); + // Overloaded operator for value access decimal& operator[] (int index); @@ -221,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const { mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]); } +// Normalize the vector +inline void Vector3::normalize() { + decimal l = length(); + assert(l != 0.0); + mValues[0] /= l; + mValues[1] /= l; + mValues[2] /= l; +} + // Return the corresponding absolute value vector inline Vector3 Vector3::getAbsoluteVector() const { return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2])); @@ -287,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) { return *this; } +// Overloaded operator for division by a number with assignment +inline Vector3& Vector3::operator/=(decimal number) { + mValues[0] /= number; + mValues[1] /= number; + mValues[2] /= number; + return *this; +} + // Overloaded operator for value access inline decimal& Vector3::operator[] (int index) { return mValues[index];