From aa236286de76d380cb77dd0ff237b48c952989b8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 19 Feb 2013 23:16:48 +0100 Subject: [PATCH] Remove files --- src/engine/ConstraintSolver.cpp | 835 -------------------------------- src/engine/ConstraintSolver.h | 424 ---------------- 2 files changed, 1259 deletions(-) delete mode 100644 src/engine/ConstraintSolver.cpp delete mode 100644 src/engine/ConstraintSolver.h diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp deleted file mode 100644 index 8e81b266..00000000 --- a/src/engine/ConstraintSolver.cpp +++ /dev/null @@ -1,835 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 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 "ConstraintSolver.h" -#include "DynamicsWorld.h" -#include "../body/RigidBody.h" - -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), mIsSolveFrictionAtContactManifoldCenterActive(true) { - -} - -// Destructor -ConstraintSolver::~ConstraintSolver() { - -} - -// Initialize the constraint solver -void ConstraintSolver::initialize() { - - nbConstraints = 0; - - // TODO : Use better allocation here - mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; - - mNbContactConstraints = 0; - - // For each contact manifold of the world - vector::iterator it; - for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) { - ContactManifold contactManifold = *it; - - ContactConstraint& constraint = mContactConstraints[mNbContactConstraints]; - - assert(contactManifold.nbContacts > 0); - - RigidBody* body1 = contactManifold.contacts[0]->getBody1(); - RigidBody* body2 = contactManifold.contacts[0]->getBody2(); - - // Fill in the body number maping - mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); - mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); - - // Add the two bodies of the constraint in the constraintBodies list - mConstraintBodies.insert(body1); - mConstraintBodies.insert(body2); - - Vector3 x1 = body1->getTransform().getPosition(); - Vector3 x2 = body2->getTransform().getPosition(); - - constraint.indexBody1 = mMapBodyToIndex[body1]; - constraint.indexBody2 = mMapBodyToIndex[body2]; - constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - constraint.isBody1Moving = body1->getIsMotionEnabled(); - constraint.isBody2Moving = body2->getIsMotionEnabled(); - constraint.massInverseBody1 = body1->getMassInverse(); - 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; cgetWorldPointOnBody1(); - Vector3 p2 = contact->getWorldPointOnBody2(); - - contactPointConstraint.contact = contact; - contactPointConstraint.normal = contact->getNormal(); - contactPointConstraint.r1 = p1 - x1; - contactPointConstraint.r2 = p2 - x2; - contactPointConstraint.penetrationDepth = contact->getPenetrationDepth(); - contactPointConstraint.isRestingContact = contact->getIsRestingContact(); - contact->setIsRestingContact(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++; - } - - // Compute the number of bodies that are part of some active constraint - nbBodies = mConstraintBodies.size(); - - mLinearVelocities = new Vector3[nbBodies]; - mAngularVelocities = new Vector3[nbBodies]; - mSplitLinearVelocities = new Vector3[nbBodies]; - mSplitAngularVelocities = new Vector3[nbBodies]; - - assert(mMapBodyToIndex.size() == nbBodies); -} - -// Initialize the constrained bodies -void ConstraintSolver::initializeBodies() { - - // For each current body that is implied in some constraint - RigidBody* rigidBody; - for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { - rigidBody = *it; - uint bodyNumber = mMapBodyToIndex[rigidBody]; - - // TODO : Use polymorphism and remove this downcasting - assert(rigidBody); - - mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); - mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); - } -} - -// Initialize the contact constraints before solving the system -void ConstraintSolver::initializeContactConstraints() { - - // For each contact constraint - for (uint c=0; c 0.0 ? contact.inversePenetrationMass = 1.0 / massPenetration : 0.0; - - 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; - } - - // 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 - // under a given threshold), we don't add a restitution velocity bias - contact.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contact.normal); - // TODO : Use a constant here - if (deltaVDotN < 1.0f) { - contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; - } - - // Get the cached lambda values of the constraint - 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; - } - } -} - -// 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 ConstraintSolver::warmStart() { - - // For each constraint - for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contact.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contact.restitutionBias; - - // Compute the Lagrange multiplier - if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass; - } - else { - deltaLambda = - (Jv + b) * contact.inversePenetrationMass; - } - lambdaTemp = contact.penetrationImpulse; - contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f); - deltaLambda = contact.penetrationImpulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contact.normal * deltaLambda; - Vector3 angularImpulseBody1 = -contact.r1CrossN * deltaLambda; - Vector3 linearImpulseBody2 = contact.normal * deltaLambda; - Vector3 angularImpulseBody2 = contact.r2CrossN * deltaLambda; - const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // 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) { - - // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1); - decimal JvSplit = deltaVSplit.dot(contact.normal); - decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass; - decimal lambdaTempSplit = contact.penetrationSplitImpulse; - contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); - deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.normal * deltaLambdaSplit; - angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit; - linearImpulseBody2 = contact.normal * deltaLambdaSplit; - angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit; - const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - applySplitImpulse(splitImpulsePenetration, constraint); - } - - // 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 - Vector3 deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); - decimal Jv = deltaV.dot(constraint.frictionVector1); - - 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 - 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); - - // ------ Second friction constraint at the center of the contact manifol ----- // - - // Compute J*v - deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); - Jv = deltaV.dot(constraint.frictionVector2); - - 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 = -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); - } - } - } -} - -// Solve the constraints -void ConstraintSolver::solve(decimal timeStep) { - - mTimeStep = timeStep; - - // Initialize the solver - initialize(); - - initializeBodies(); - - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); - - // Warm start the solver - if (mIsWarmStartingActive) { - warmStart(); - } - - // Solve the contact constraints - solveContactConstraints(); - - // Cache the lambda values in order to use them in the next step - storeImpulses(); -} - -// Store the computed impulses to use them to -// warm start the solver at the next iteration -void ConstraintSolver::storeImpulses() { - - // For each constraint - for (uint c=0; csetCachedLambda(0, contact.penetrationImpulse); - contact.contact->setCachedLambda(1, contact.friction1Impulse); - contact.contact->setCachedLambda(2, contact.friction2Impulse); - - 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; - } -} - -// Apply an impulse to the two bodies of a constraint -void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) { - - // Update the velocities of the bodies by applying the impulse P - if (constraint.isBody1Moving) { - mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * - impulse.linearImpulseBody1; - mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - } - if (constraint.isBody2Moving) { - mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * - impulse.linearImpulseBody2; - mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; - } -} - -// Apply an impulse to the two bodies of a constraint -void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) { - - // Update the velocities of the bodies by applying the impulse P - if (constraint.isBody1Moving) { - mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * - impulse.linearImpulseBody1; - mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - } - if (constraint.isBody2Moving) { - mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; - } -} - -// 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 ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointConstraint& contact) const { - - // Update the old friction vectors - //contact.oldFrictionVector1 = contact.frictionVector1; - //contact.oldFrictionVector2 = contact.frictionVector2; - - 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(); -} - -// 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 deleted file mode 100644 index 0125dd5e..00000000 --- a/src/engine/ConstraintSolver.h +++ /dev/null @@ -1,424 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 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 CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H - -// Libraries -#include "../constraint/Contact.h" -#include "ContactManifold.h" -#include "../configuration.h" -#include "../constraint/Constraint.h" -#include -#include - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Declarations -class DynamicsWorld; - -// Structure Impulse -struct Impulse { - - public: - const Vector3& linearImpulseBody1; - const Vector3& linearImpulseBody2; - const Vector3& angularImpulseBody1; - const Vector3& angularImpulseBody2; - - // Constructor - Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, - const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) - : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), - linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { - - } -}; - -// Structure ContactPointConstraint -// Internal structure for a contact point constraint -struct ContactPointConstraint { - - decimal penetrationImpulse; // Accumulated normal impulse - decimal friction1Impulse; // Accumulated impulse in the 1st friction direction - decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction - decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction - Vector3 normal; // Normal vector of the contact - Vector3 frictionVector1; // First friction vector in the tangent plane - Vector3 frictionVector2; // Second friction vector in the tangent plane - Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane - Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane - Vector3 r1; // Vector from the body 1 center to the contact point - Vector3 r2; // Vector from the body 2 center to the contact point - 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 - Vector3 r1CrossN; // Cross product of r1 with the contact normal - Vector3 r2CrossN; // Cross product of r2 with the contact normal - decimal penetrationDepth; // Penetration depth - decimal restitutionBias; // Velocity restitution bias - decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration - decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction - decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction - bool isRestingContact; // True if the contact was existing last time step - Contact* contact; // TODO : REMOVE THIS -}; - -// Structure ContactConstraint -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 - 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 -}; - -/* ------------------------------------------------------------------- - Class ConstrainSolver : - 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. - - ------------------------------------------------------------------- -*/ -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 - std::vector activeConstraints; // Current active constraints in the physics world - bool isErrorCorrectionActive; // True if error correction (with world order) is active - uint mNbIterations; // Number of iterations of the LCP solver - uint nbConstraints; // Total number of constraints (with the auxiliary constraints) - uint nbBodies; // Current number of bodies in the physics world - RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference - // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains - Vector3* mLinearVelocities; // Array of constrained linear velocities - Vector3* mAngularVelocities; // Array of constrained angular velocities - - // Split linear velocities for the position contact solver (split impulse) - Vector3* mSplitLinearVelocities; - - // Split angular velocities for the position contact solver (split impulse) - Vector3* mSplitAngularVelocities; - - decimal mTimeStep; // Current time step - - // Contact constraints - ContactConstraint* mContactConstraints; - - // Number of contact constraints - uint mNbContactConstraints; - - // Constrained bodies - std::set mConstraintBodies; - - // Map body to index - std::map mMapBodyToIndex; - - // 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 constraint solver - void initialize(); - - // Initialize the constrained bodies - void initializeBodies(); - - // Initialize the contact constraints before solving the system - void initializeContactConstraints(); - - // Store the computed impulses to use them to - // warm start the solver at the next iteration - void storeImpulses(); - - // Warm start the solver - void warmStart(); - - // Solve the contact constraints by applying sequential impulses - void solveContactConstraints(); - - // Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); - - // Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint); - - // 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 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 -------------------- // - - // Constructor - ConstraintSolver(DynamicsWorld* world); - - // Destructor - virtual ~ConstraintSolver(); - - // Solve the constraints - void solve(decimal timeStep); - - // Return true if the body is in at least one constraint - bool isConstrainedBody(RigidBody* body) const; - - // Return the constrained linear velocity of a body after solving the constraints - Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); - - // Return the split linear velocity - Vector3 getSplitLinearVelocityOfBody(RigidBody* body); - - // Return the constrained angular velocity of a body after solving the constraints - Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); - - // Return the split angular velocity - Vector3 getSplitAngularVelocityOfBody(RigidBody* body); - - // Clean up the constraint solver - void cleanup(); - - // 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 -inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { - return mConstraintBodies.count(body) == 1; -} - -// Return the constrained linear velocity of a body after solving the constraints -inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mLinearVelocities[indexBody]; -} - -// Return the split linear velocity -inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mSplitLinearVelocities[indexBody]; -} - -// Return the constrained angular velocity of a body after solving the constraints -inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mAngularVelocities[indexBody]; -} - -// Return the split angular velocity -inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mSplitAngularVelocities[indexBody]; -} - -// Clean up the constraint solver -inline void ConstraintSolver::cleanup() { - mMapBodyToIndex.clear(); - mConstraintBodies.clear(); - activeConstraints.clear(); - - if (mContactConstraints != 0) { - delete[] mContactConstraints; - mContactConstraints = 0; - } - if (mLinearVelocities != 0) { - delete[] mLinearVelocities; - mLinearVelocities = 0; - } - if (mAngularVelocities != 0) { - delete[] mAngularVelocities; - mAngularVelocities = 0; - } -} - -// 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 { - decimal restitution1 = body1->getRestitution(); - decimal restitution2 = body2->getRestitution(); - - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; -} - -} // End of ReactPhysics3D namespace - -#endif