From 7b19ce4ef8aec7a35fc2c895b63c207d7d79199e Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Mon, 5 Jul 2010 20:52:16 +0000 Subject: [PATCH] Collision response done git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@344 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../engine/ConstraintSolver.cpp | 117 ++++--------- .../reactphysics3d/engine/ConstraintSolver.h | 44 ++++- .../reactphysics3d/engine/PhysicsEngine.cpp | 159 ++++++++++++------ sources/reactphysics3d/engine/PhysicsEngine.h | 8 +- 4 files changed, 179 insertions(+), 149 deletions(-) diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 0e424113..f6aa27aa 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -21,19 +21,18 @@ #include "ConstraintSolver.h" #include "../mathematics/lcp/LCPProjectedGaussSeidel.h" #include "../body/RigidBody.h" -#include "../integration/SemiImplicitEuler.h" // TODO : Delete this using namespace reactphysics3d; // Constructor ConstraintSolver::ConstraintSolver(PhysicsWorld* world) :physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) { - integrationAlgorithm = new SemiImplicitEuler(); + } // Destructor ConstraintSolver::~ConstraintSolver() { - delete integrationAlgorithm; + } // Allocate all the matrices needed to solve the LCP problem @@ -85,7 +84,8 @@ void ConstraintSolver::allocate() { lowerBounds.changeSize(nbConstraints); upperBounds.changeSize(nbConstraints); Minv_sp = new Matrix[nbBodies]; - V = new Vector[nbBodies]; + V1 = new Vector[nbBodies]; + Vconstraint = new Vector[nbBodies]; Fext = new Vector[nbBodies]; } @@ -157,11 +157,15 @@ void ConstraintSolver::fillInMatrices() { rigidBody = dynamic_cast(body); assert(rigidBody != 0); - // Compute the vector with velocities values + // Compute the vector V1 with initial velocities values v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity()); v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity()); - V[bodyNumber].changeSize(6); - V[bodyNumber] = v; + V1[bodyNumber].changeSize(6); + V1[bodyNumber] = v; + + // Compute the vector Vconstraint with final constraint velocities + Vconstraint[bodyNumber].changeSize(6); + Vconstraint[bodyNumber].initWithValue(0.0); // Compute the vector with forces and torques values f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce()); @@ -172,8 +176,10 @@ void ConstraintSolver::fillInMatrices() { // Compute the inverse sparse mass matrix Matrix mInv(6,6); mInv.initWithValue(0.0); - mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); - mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); + if (rigidBody->getIsMotionEnabled()) { + mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); + mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); + } Minv_sp[bodyNumber].changeSize(6, 6); Minv_sp[bodyNumber] = mInv; } @@ -197,7 +203,8 @@ void ConstraintSolver::freeMemory() { delete[] B_sp[1]; delete[] B_sp; delete[] Minv_sp; - delete[] V; + delete[] V1; + delete[] Vconstraint; delete[] Fext; } @@ -209,12 +216,11 @@ void ConstraintSolver::computeVectorB(double dt) { b = errorValues * oneOverDT; for (uint c = 0; csetLambdaInit(lambdaInit); lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda); - // Compute the vector V2 - computeVectorV(dt); - - // Update the velocity of each body - // TODO : Put this code somewhere else - for (std::set::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++) { - RigidBody* body = dynamic_cast(*it); - //std::cout << "Velocity Y before : " << body->getCurrentBodyState().getLinearVelocity().getY() << std::endl; - //std::cout << "Velocity Y after : " << V[bodyNumberMapping[constraintBodies.at(i)]].getValue(1) << std::endl; - } - - for (std::vector::iterator it = physicsWorld->getBodiesBeginIterator(); it != physicsWorld->getBodiesEndIterator(); it++) { - // If this is a not constrained body - if (bodyNumberMapping.find(*it) == bodyNumberMapping.end()) { - RigidBody* rigidBody = dynamic_cast(*it); - - // The current body state of the body becomes the previous body state - rigidBody->updatePreviousBodyState(); - - // Integrate the current body state at time t to get the next state at time t + dt - integrationAlgorithm->integrate(rigidBody->getCurrentBodyState(), dt, dt); - - // If the body state has changed, we have to update some informations in the rigid body - rigidBody->update(); - } - else { - RigidBody* rigidBody = dynamic_cast(*it); - - // If the gravity force is on - /* - if(physicsWorld->getIsGravityOn()) { - // Apply the current gravity force to the body - rigidBody->getCurrentBodyState().setExternalForce(physicsWorld->getGravity()); - } - */ - - // The current body state of the body becomes the previous body state - rigidBody->updatePreviousBodyState(); - - const Vector newLinVelocity = V[bodyNumberMapping[*it]].getSubVector(0, 3); - const Vector newAngVelocity = V[bodyNumberMapping[*it]].getSubVector(3, 3); - const Vector3D linVel(newLinVelocity.getValue(0), newLinVelocity.getValue(1), newLinVelocity.getValue(2)); - const Vector3D angVel(newAngVelocity.getValue(0), newAngVelocity.getValue(1), newAngVelocity.getValue(2)); - BodyState& bodyState = rigidBody->getCurrentBodyState(); - rigidBody->getCurrentBodyState().setLinearVelocity(linVel); - rigidBody->getCurrentBodyState().setAngularVelocity(angVel); - - // Normalize the orientation quaternion - rigidBody->getCurrentBodyState().setOrientation(rigidBody->getCurrentBodyState().getOrientation().getUnit()); - - // Compute the spin quaternion - const Vector3D angularVelocity = rigidBody->getCurrentBodyState().getAngularVelocity(); - rigidBody->getCurrentBodyState().setSpin(Quaternion(angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5); - - bodyState.setPosition(bodyState.getPosition() + bodyState.getLinearVelocity() * dt); - bodyState.setOrientation(bodyState.getOrientation() + bodyState.getSpin() * dt); - - // If the body state has changed, we have to update some informations in the rigid body - rigidBody->update(); - } - } - - freeMemory(); + // Compute the vector Vconstraint + computeVectorVconstraint(dt); } diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 6b9133b5..a0966588 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/sources/reactphysics3d/engine/ConstraintSolver.h @@ -33,6 +33,7 @@ namespace reactphysics3d { // Constants +// TODO : Set this to 10 after debugging const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem /* ------------------------------------------------------------------- @@ -43,7 +44,6 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol */ class ConstraintSolver { protected: - IntegrationAlgorithm* integrationAlgorithm; // TODO : Delete this PhysicsWorld* physicsWorld; // Reference to the physics world LCPSolver* lcpSolver; // LCP Solver std::vector activeConstraints; // Current active constraints in the physics world @@ -68,23 +68,51 @@ class ConstraintSolver { Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body // This is an array of size nbBodies that contains in each cell a 6x6 matrix - Vector* V; // Array that contains for each body the Vector that contains linear and angular velocities + Vector* V1; // Array that contains for each body the Vector that contains linear and angular velocities // Each cell contains a 6x1 vector with linear and angular velocities + Vector* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities Vector* Fext; // Array that contains for each body the vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. void allocate(); // Allocate all the matrices needed to solve the LCP problem void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem - void freeMemory(); // Free the memory that was allocated in the allocate() method void computeVectorB(double dt); // Compute the vector b void computeMatrixB_sp(); // Compute the matrix B_sp - void computeVectorV(double dt); // Compute the vector V2 + void computeVectorVconstraint(double dt); // Compute the vector V2 public: - ConstraintSolver(PhysicsWorld* world); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(double dt); // Solve the current LCP problem + ConstraintSolver(PhysicsWorld* world); // Constructor + virtual ~ConstraintSolver(); // Destructor + void solve(double dt); // Solve the current LCP problem + bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint + Vector3D getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem + Vector3D getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem + void freeMemory(); // Free the memory that was allocated in the allocate() method }; +// Return true if the body is in at least one constraint +inline bool ConstraintSolver::isConstrainedBody(Body* body) const { + if(constraintBodies.find(body) != constraintBodies.end()) { + return true; + } + return false; +} + +// Return the constrained linear velocity of a body after solving the LCP problem +inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { + assert(isConstrainedBody(body)); + Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(0, 3); + return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); + +} + +// Return the constrained angular velocity of a body after solving the LCP problem +inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { + assert(isConstrainedBody(body)); + Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(3, 3); + return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); +} + + } // End of ReactPhysics3D namespace -#endif +#endif \ No newline at end of file diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index b20ed074..85f061e6 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp @@ -46,6 +46,7 @@ void PhysicsEngine::update() { updateCollision(); } +/* // TODO : Delete this method // Update the physics simulation void PhysicsEngine::updateDynamic() { @@ -59,7 +60,7 @@ void PhysicsEngine::updateDynamic() { RigidBody* rigidBody = dynamic_cast(*it); if (rigidBody && rigidBody->getIsMotionEnabled()) { // Update the state of the rigid body - updateBodyState(rigidBody, timer.getTimeStep()); + //updateBodyState(rigidBody, timer.getTimeStep()); } } @@ -79,86 +80,144 @@ void PhysicsEngine::updateDynamic() { } } } +*/ // TODO : Delethe this method // Update the physics simulation void PhysicsEngine::updateCollision() { + bool existCollision = false; + + // Apply the gravity force to all bodies + applyGravity(); // While the time accumulator is not empty while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { + existCollision = false; // Compute the collision detection if (collisionDetection.computeCollisionDetection()) { - // TODO : Delete this ---------------------------------------------------------- - for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) { - Contact* contact = dynamic_cast(*it); - std::cout << "Const : " << contact << "pDepth before: " << contact->getPenetrationDepth() << std::endl; - } - /* - for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { - RigidBody* rigidBody1 = dynamic_cast((*it)->getBody1()); - RigidBody* rigidBody2 = dynamic_cast((*it)->getBody2()); - rigidBody1->setIsMotionEnabled(false); - rigidBody2->setIsMotionEnabled(false); - } - */ - // ----------------------------------------------------------------------------- + existCollision = true; // Solve constraints constraintSolver.solve(timer.getTimeStep().getValue()); } - else { - for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { - // If the body is a RigidBody and if the rigid body motion is enabled - RigidBody* rigidBody = dynamic_cast(*it); - if (rigidBody && rigidBody->getIsMotionEnabled()) { - // Update the state of the rigid body with an entire time step - updateBodyState(rigidBody, timer.getTimeStep()); - } - } - } - - // TODO : Delete this - collisionDetection.computeCollisionDetection(); - for (std::vector::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) { - Contact* contact = dynamic_cast(*it); - std::cout << "Const : " << contact << "pDepth after: " << contact->getPenetrationDepth() << std::endl; - RigidBody* rigidBody1 = dynamic_cast(contact->getBody1()); - RigidBody* rigidBody2 = dynamic_cast(contact->getBody2()); - rigidBody1->getCurrentBodyState().setPosition(rigidBody1->getCurrentBodyState().getPosition() - contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4); - rigidBody2->getCurrentBodyState().setPosition(rigidBody2->getCurrentBodyState().getPosition() + contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4); - } // Update the timer timer.update(); + + // Update the position and orientation of each body + updateAllBodiesMotion(); + + // Free the allocated memory of the constraint solver + if (existCollision) { + constraintSolver.freeMemory(); + } } + /* // For each body in the the dynamic world for(std::vector::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { // If the body is a RigidBody and if the rigid body motion is enabled RigidBody* rigidBody = dynamic_cast(*it); if (rigidBody) { - // Update the interpolation factor of the rigid body - // This one will be used to compute the interpolated state - rigidBody->setInterpolationFactor(timer.getInterpolationFactor()); + } } + */ +} + +// Compute the motion of all bodies and update their positions and orientations +// First this method compute the vector V2 = V_constraint + V_forces + V1 where +// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext) +// V2 is the final velocity after the timestep and V1 is the velocity before the +// timestep. +// After having computed the velocity V2, this method will update the position +// and orientation of each body. +// This method uses the semi-implicit Euler method to update the position and +// orientation of the body +void PhysicsEngine::updateAllBodiesMotion() { + double dt = timer.getTimeStep().getValue(); + Vector3D newLinearVelocity; + Vector3D newAngularVelocity; + + // For each body of thephysics world + for (std::vector::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + + RigidBody* rigidBody = dynamic_cast(*it); + assert(rigidBody); + + // If the body is able to move + if (rigidBody->getIsMotionEnabled()) { + newLinearVelocity.setAllValues(0.0, 0.0, 0.0); + newAngularVelocity.setAllValues(0.0, 0.0, 0.0); + + // If it's a constrained body + if (constraintSolver.isConstrainedBody(*it)) { + // Get the constrained linear and angular velocities from the constraint solver + newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it); + newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it); + } + + // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the + // external forces and torques. + newLinearVelocity = newLinearVelocity + dt * rigidBody->getCurrentBodyState().getMassInverse().getValue() * rigidBody->getCurrentBodyState().getExternalForce(); + newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getCurrentBodyState().getExternalTorque(); + + // Add the velocity V1 to the new velocity + newLinearVelocity = newLinearVelocity + rigidBody->getCurrentBodyState().getLinearVelocity(); + newAngularVelocity = newAngularVelocity + rigidBody->getCurrentBodyState().getAngularVelocity(); + + // Update the position and the orientation of the body according to the new velocity + updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); + + // If the body state has changed, we have to update some informations in the rigid body + rigidBody->update(); + } + + // Update the interpolation factor of the rigid body + // This one will be used to compute the interpolated state + rigidBody->setInterpolationFactor(timer.getInterpolationFactor()); + } } -// Update the state of a rigid body -void PhysicsEngine::updateBodyState(RigidBody* const rigidBody, const Time& timeStep) { +// Update the position and orientation of a body +// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new +// orientation of the body + void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) { + double dt = timer.getTimeStep().getValue(); - // If the gravity force is on - if(world->getIsGravityOn()) { - // Apply the current gravity force to the body - rigidBody->getCurrentBodyState().setExternalForce(world->getGravity()); - } + RigidBody* rigidBody = dynamic_cast(body); + assert(rigidBody); // The current body state of the body becomes the previous body state rigidBody->updatePreviousBodyState(); - // Integrate the current body state at time t to get the next state at time t + dt - integrationAlgorithm->integrate(rigidBody->getCurrentBodyState(), timer.getTime(), timeStep); + BodyState& bodyState = rigidBody->getCurrentBodyState(); - // If the body state has changed, we have to update some informations in the rigid body - rigidBody->update(); + // Normalize the orientation quaternion + bodyState.setOrientation(bodyState.getOrientation().getUnit()); + + // Update the linear and angular velocity of the body + bodyState.setLinearVelocity(newLinVelocity); + bodyState.setAngularVelocity(newAngVelocity); + + // Update the position and the orientation of the body + bodyState.setPosition(bodyState.getPosition() + newLinVelocity * dt); + bodyState.setOrientation(bodyState.getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5 * dt); +} + +// Apply the gravity force to all bodies of the physics world +void PhysicsEngine::applyGravity() { + + // For each body of the physics world + for (std::vector::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) { + + RigidBody* rigidBody = dynamic_cast(*it); + assert(rigidBody); + + // If the gravity force is on + if(world->getIsGravityOn()) { + // Apply the current gravity force to the body + rigidBody->getCurrentBodyState().setExternalForce(world->getGravity()); + } + } } diff --git a/sources/reactphysics3d/engine/PhysicsEngine.h b/sources/reactphysics3d/engine/PhysicsEngine.h index 62baecc6..19bbc786 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.h +++ b/sources/reactphysics3d/engine/PhysicsEngine.h @@ -45,8 +45,10 @@ class PhysicsEngine { CollisionDetection collisionDetection; // Collision detection ConstraintSolver constraintSolver; // Constraint solver - void updateBodyState(RigidBody* const rigidBody, const Time& timeStep); // Update the state of a rigid body - + void updateBodyState(RigidBody* const rigidBody, const Time& timeStep); // Update the state of a rigid body // TODO : Delete this + void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations + void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body + void applyGravity(); // Apply the gravity force to all bodies public : PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument); // Constructor ~PhysicsEngine(); // Destructor @@ -54,7 +56,7 @@ class PhysicsEngine { void start(); // Start the physics simulation void stop(); // Stop the physics simulation void update(); // Update the physics simulation - void updateDynamic(); // TODO : Delete this method + //void updateDynamic(); // TODO : Delete this method void updateCollision(); // TODO : Delete this collision void initializeDisplayTime(const Time& displayTime); // Initialize the display time void updateDisplayTime(const Time& newDisplayTime); // Update the display time