mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Collision response done

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@344 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-07-05 20:52:16 +00:00
parent 57b2448111
commit 7b19ce4ef8
4 changed files with 179 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -21,19 +21,18 @@
#include "ConstraintSolver.h" #include "ConstraintSolver.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h" #include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
#include "../integration/SemiImplicitEuler.h" // TODO : Delete this
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world) ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) { :physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
integrationAlgorithm = new SemiImplicitEuler();
} }
// Destructor // Destructor
ConstraintSolver::~ConstraintSolver() { ConstraintSolver::~ConstraintSolver() {
delete integrationAlgorithm;
} }
// Allocate all the matrices needed to solve the LCP problem // Allocate all the matrices needed to solve the LCP problem
@ -85,7 +84,8 @@ void ConstraintSolver::allocate() {
lowerBounds.changeSize(nbConstraints); lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints); upperBounds.changeSize(nbConstraints);
Minv_sp = new Matrix[nbBodies]; Minv_sp = new Matrix[nbBodies];
V = new Vector[nbBodies]; V1 = new Vector[nbBodies];
Vconstraint = new Vector[nbBodies];
Fext = new Vector[nbBodies]; Fext = new Vector[nbBodies];
} }
@ -157,11 +157,15 @@ void ConstraintSolver::fillInMatrices() {
rigidBody = dynamic_cast<RigidBody*>(body); rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody != 0); 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(0, rigidBody->getCurrentBodyState().getLinearVelocity());
v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity()); v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
V[bodyNumber].changeSize(6); V1[bodyNumber].changeSize(6);
V[bodyNumber] = v; 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 // Compute the vector with forces and torques values
f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce()); f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
@ -172,8 +176,10 @@ void ConstraintSolver::fillInMatrices() {
// Compute the inverse sparse mass matrix // Compute the inverse sparse mass matrix
Matrix mInv(6,6); Matrix mInv(6,6);
mInv.initWithValue(0.0); mInv.initWithValue(0.0);
if (rigidBody->getIsMotionEnabled()) {
mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
}
Minv_sp[bodyNumber].changeSize(6, 6); Minv_sp[bodyNumber].changeSize(6, 6);
Minv_sp[bodyNumber] = mInv; Minv_sp[bodyNumber] = mInv;
} }
@ -197,7 +203,8 @@ void ConstraintSolver::freeMemory() {
delete[] B_sp[1]; delete[] B_sp[1];
delete[] B_sp; delete[] B_sp;
delete[] Minv_sp; delete[] Minv_sp;
delete[] V; delete[] V1;
delete[] Vconstraint;
delete[] Fext; delete[] Fext;
} }
@ -209,12 +216,11 @@ void ConstraintSolver::computeVectorB(double dt) {
b = errorValues * oneOverDT; b = errorValues * oneOverDT;
for (uint c = 0; c<nbConstraints; c++) { for (uint c = 0; c<nbConstraints; c++) {
int size1 = J_sp[c][0].getNbColumn(); // TODO : Delete this
// Substract 1.0/dt*J*V to the vector b // Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - (J_sp[c][0] * V[indexBody1]).getValue(0,0) * oneOverDT); b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]).getValue(0,0) * oneOverDT);
b.setValue(c, b.getValue(c) - (J_sp[c][1] * V[indexBody2]).getValue(0,0) * oneOverDT); b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]).getValue(0,0) * oneOverDT);
// Substract J*M^-1*F_ext to the vector b // Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1] b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
@ -237,26 +243,21 @@ void ConstraintSolver::computeMatrixB_sp() {
} }
} }
// Compute the vector V2 according to the formula // Compute the vector V_constraint (which corresponds to the constraint part of
// V2 = dt * (M^-1 * J^T * lambda + M^-1 * F_ext) + V1 // the final V2 vector) according to the formula
// Note that we use the vector V to store both V1 and V2 and that at the beginning // V_constraint = dt * (M^-1 * J^T * lambda)
// of this method, the vector V already contains the vector V1. // Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B. // Note that M^-1 * J^T = B.
// This method is called after that the LCP solver have computed lambda // This method is called after that the LCP solver have computed lambda
void ConstraintSolver::computeVectorV(double dt) { void ConstraintSolver::computeVectorVconstraint(double dt) {
uint indexBody1, indexBody2; uint indexBody1, indexBody2;
// Compute dt * (M^-1 * J^T * lambda // Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<nbConstraints; i++) { for (uint i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]]; indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]]; indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt; Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
V[indexBody2] = V[indexBody2] + (B_sp[indexBody2][i] * lambda.getValue(i)).getVector() * dt; Vconstraint[indexBody2] = Vconstraint[indexBody2] +(B_sp[indexBody2][i] * lambda.getValue(i)).getVector() * dt;
}
// Compute dt * (M^-1 * F_ext)
for (uint i=0; i<nbBodies; i++) {
V[i] = V[i] + (Minv_sp[i] * Fext[i]).getVector() * dt;
} }
} }
@ -276,70 +277,10 @@ void ConstraintSolver::solve(double dt) {
// Solve the LCP problem (computation of lambda) // Solve the LCP problem (computation of lambda)
Vector lambdaInit(nbConstraints); Vector lambdaInit(nbConstraints);
lambdaInit.initWithValue(0.0);
lcpSolver->setLambdaInit(lambdaInit); lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda); lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Compute the vector V2 // Compute the vector Vconstraint
computeVectorV(dt); computeVectorVconstraint(dt);
// Update the velocity of each body
// TODO : Put this code somewhere else
for (std::set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++) {
RigidBody* body = dynamic_cast<RigidBody*>(*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<Body*>::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<RigidBody*>(*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<RigidBody*>(*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();
} }

36
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -33,6 +33,7 @@
namespace reactphysics3d { namespace reactphysics3d {
// Constants // Constants
// TODO : Set this to 10 after debugging
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem 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 { class ConstraintSolver {
protected: protected:
IntegrationAlgorithm* integrationAlgorithm; // TODO : Delete this
PhysicsWorld* physicsWorld; // Reference to the physics world PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> 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 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 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 // 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 // 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 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. // Each cell contains a 6x1 vector with external force and torque.
void allocate(); // Allocate all the matrices needed to solve the LCP problem 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 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 computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp 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: public:
ConstraintSolver(PhysicsWorld* world); // Constructor ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem 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 } // End of ReactPhysics3D namespace
#endif #endif

153
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

@ -46,6 +46,7 @@ void PhysicsEngine::update() {
updateCollision(); updateCollision();
} }
/*
// TODO : Delete this method // TODO : Delete this method
// Update the physics simulation // Update the physics simulation
void PhysicsEngine::updateDynamic() { void PhysicsEngine::updateDynamic() {
@ -59,7 +60,7 @@ void PhysicsEngine::updateDynamic() {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it); RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody && rigidBody->getIsMotionEnabled()) { if (rigidBody && rigidBody->getIsMotionEnabled()) {
// Update the state of the rigid body // 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 // TODO : Delethe this method
// Update the physics simulation // Update the physics simulation
void PhysicsEngine::updateCollision() { void PhysicsEngine::updateCollision() {
bool existCollision = false;
// Apply the gravity force to all bodies
applyGravity();
// While the time accumulator is not empty // While the time accumulator is not empty
while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { while(timer.getAccumulator() >= timer.getTimeStep().getValue()) {
existCollision = false;
// Compute the collision detection // Compute the collision detection
if (collisionDetection.computeCollisionDetection()) { if (collisionDetection.computeCollisionDetection()) {
// TODO : Delete this ---------------------------------------------------------- existCollision = true;
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) {
Contact* contact = dynamic_cast<Contact*>(*it);
std::cout << "Const : " << contact << "pDepth before: " << contact->getPenetrationDepth() << std::endl;
}
/*
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>((*it)->getBody1());
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>((*it)->getBody2());
rigidBody1->setIsMotionEnabled(false);
rigidBody2->setIsMotionEnabled(false);
}
*/
// -----------------------------------------------------------------------------
// Solve constraints // Solve constraints
constraintSolver.solve(timer.getTimeStep().getValue()); constraintSolver.solve(timer.getTimeStep().getValue());
} }
else {
for(std::vector<Body*>::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<RigidBody*>(*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<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) {
Contact* contact = dynamic_cast<Contact*>(*it);
std::cout << "Const : " << contact << "pDepth after: " << contact->getPenetrationDepth() << std::endl;
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(contact->getBody1());
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(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 // Update the timer
timer.update(); 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 each body in the the dynamic world
for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
// If the body is a RigidBody and if the rigid body motion is enabled // If the body is a RigidBody and if the rigid body motion is enabled
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it); RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody) { if (rigidBody) {
}
}
*/
}
// 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<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*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 // Update the interpolation factor of the rigid body
// This one will be used to compute the interpolated state // This one will be used to compute the interpolated state
rigidBody->setInterpolationFactor(timer.getInterpolationFactor()); rigidBody->setInterpolationFactor(timer.getInterpolationFactor());
} }
} }
// 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();
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody);
// The current body state of the body becomes the previous body state
rigidBody->updatePreviousBodyState();
BodyState& bodyState = rigidBody->getCurrentBodyState();
// 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);
} }
// Update the state of a rigid body // Apply the gravity force to all bodies of the physics world
void PhysicsEngine::updateBodyState(RigidBody* const rigidBody, const Time& timeStep) { void PhysicsEngine::applyGravity() {
// For each body of the physics world
for (std::vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
// If the gravity force is on // If the gravity force is on
if(world->getIsGravityOn()) { if(world->getIsGravityOn()) {
// Apply the current gravity force to the body // Apply the current gravity force to the body
rigidBody->getCurrentBodyState().setExternalForce(world->getGravity()); rigidBody->getCurrentBodyState().setExternalForce(world->getGravity());
} }
}
// 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);
// If the body state has changed, we have to update some informations in the rigid body
rigidBody->update();
} }

8
sources/reactphysics3d/engine/PhysicsEngine.h
View File

@ -45,8 +45,10 @@ class PhysicsEngine {
CollisionDetection collisionDetection; // Collision detection CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver 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 : public :
PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument); // Constructor PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument); // Constructor
~PhysicsEngine(); // Destructor ~PhysicsEngine(); // Destructor
@ -54,7 +56,7 @@ class PhysicsEngine {
void start(); // Start the physics simulation void start(); // Start the physics simulation
void stop(); // Stop the physics simulation void stop(); // Stop the physics simulation
void update(); // Update 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 updateCollision(); // TODO : Delete this collision
void initializeDisplayTime(const Time& displayTime); // Initialize the display time void initializeDisplayTime(const Time& displayTime); // Initialize the display time
void updateDisplayTime(const Time& newDisplayTime); // Update the display time void updateDisplayTime(const Time& newDisplayTime); // Update the display time