Collision response done
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@344 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
57b2448111
commit
7b19ce4ef8
|
|
@ -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<RigidBody*>(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; c<nbConstraints; c++) {
|
||||
int size1 = J_sp[c][0].getNbColumn(); // TODO : Delete this
|
||||
// Substract 1.0/dt*J*V to the vector b
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
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][1] * V[indexBody2]).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] * V1[indexBody2]).getValue(0,0) * oneOverDT);
|
||||
|
||||
// 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]
|
||||
|
|
@ -237,26 +243,21 @@ void ConstraintSolver::computeMatrixB_sp() {
|
|||
}
|
||||
}
|
||||
|
||||
// Compute the vector V2 according to the formula
|
||||
// V2 = dt * (M^-1 * J^T * lambda + M^-1 * F_ext) + V1
|
||||
// Note that we use the vector V to store both V1 and V2 and that at the beginning
|
||||
// of this method, the vector V already contains the vector V1.
|
||||
// Compute the vector V_constraint (which corresponds to the constraint part of
|
||||
// the final V2 vector) according to the formula
|
||||
// V_constraint = dt * (M^-1 * J^T * lambda)
|
||||
// Note that we use the vector V to store both V1 and V_constraint.
|
||||
// Note that M^-1 * J^T = B.
|
||||
// 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;
|
||||
|
||||
// Compute dt * (M^-1 * J^T * lambda
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
|
||||
V[indexBody2] = V[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;
|
||||
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
|
||||
Vconstraint[indexBody2] = Vconstraint[indexBody2] +(B_sp[indexBody2][i] * lambda.getValue(i)).getVector() * dt;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -276,70 +277,10 @@ void ConstraintSolver::solve(double dt) {
|
|||
|
||||
// Solve the LCP problem (computation of lambda)
|
||||
Vector lambdaInit(nbConstraints);
|
||||
lambdaInit.initWithValue(0.0);
|
||||
lcpSolver->setLambdaInit(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<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();
|
||||
// Compute the vector Vconstraint
|
||||
computeVectorVconstraint(dt);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<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
|
||||
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
|
||||
|
|
@ -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<RigidBody*>(*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<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);
|
||||
}
|
||||
*/
|
||||
// -----------------------------------------------------------------------------
|
||||
existCollision = true;
|
||||
|
||||
// Solve constraints
|
||||
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
|
||||
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<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) {
|
||||
// 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<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
|
||||
// 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<RigidBody*>(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<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
|
||||
|
||||
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user