Merge branch 'outside_velocities' into sequentialimpulse2
This commit is contained in:
commit
b36f2c93b1
|
@ -37,10 +37,17 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
|
|||
const decimal ContactSolver::SLOP = decimal(0.01);
|
||||
|
||||
// Constructor
|
||||
ContactSolver::ContactSolver(DynamicsWorld& world)
|
||||
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0),
|
||||
mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true),
|
||||
mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) {
|
||||
ContactSolver::ContactSolver(DynamicsWorld& world, std::vector<Vector3>& constrainedLinearVelocities,
|
||||
std::vector<Vector3>& constrainedAngularVelocities,
|
||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
|
||||
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL),
|
||||
mConstrainedLinearVelocities(constrainedLinearVelocities),
|
||||
mConstrainedAngularVelocities(constrainedAngularVelocities),
|
||||
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||
mIsWarmStartingActive(true),
|
||||
mIsSplitImpulseActive(true),
|
||||
mIsSolveFrictionAtContactManifoldCenterActive(true) {
|
||||
|
||||
}
|
||||
|
||||
|
@ -72,10 +79,6 @@ void ContactSolver::initialize() {
|
|||
RigidBody* body1 = externalContactManifold.contacts[0]->getBody1();
|
||||
RigidBody* body2 = externalContactManifold.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);
|
||||
|
@ -84,8 +87,8 @@ void ContactSolver::initialize() {
|
|||
Vector3 x1 = body1->getTransform().getPosition();
|
||||
Vector3 x2 = body2->getTransform().getPosition();
|
||||
|
||||
internalContactManifold.indexBody1 = mMapBodyToIndex[body1];
|
||||
internalContactManifold.indexBody2 = mMapBodyToIndex[body2];
|
||||
internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
|
||||
internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
|
||||
internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
||||
internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
||||
internalContactManifold.isBody1Moving = body1->getIsMotionEnabled();
|
||||
|
@ -165,17 +168,17 @@ void ContactSolver::initialize() {
|
|||
mNbContactConstraints++;
|
||||
}
|
||||
|
||||
// Compute the number of bodies that are part of some active constraint
|
||||
uint nbBodies = mConstraintBodies.size();
|
||||
|
||||
// Allocated memory for velocities
|
||||
// Allocated memory for split impulse velocities
|
||||
// TODO : Use better memory allocation here
|
||||
mLinearVelocities = new Vector3[nbBodies];
|
||||
mAngularVelocities = new Vector3[nbBodies];
|
||||
mSplitLinearVelocities = new Vector3[nbBodies];
|
||||
mSplitAngularVelocities = new Vector3[nbBodies];
|
||||
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
|
||||
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
|
||||
assert(mSplitLinearVelocities != NULL);
|
||||
assert(mSplitAngularVelocities != NULL);
|
||||
|
||||
assert(mMapBodyToIndex.size() == nbBodies);
|
||||
assert(mConstraintBodies.size() > 0);
|
||||
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
|
||||
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
|
||||
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
|
||||
}
|
||||
|
||||
// Initialize the constrained bodies
|
||||
|
@ -187,10 +190,8 @@ void ContactSolver::initializeBodies() {
|
|||
RigidBody* rigidBody = *it;
|
||||
assert(rigidBody);
|
||||
|
||||
uint bodyNumber = mMapBodyToIndex[rigidBody];
|
||||
uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
|
||||
|
||||
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);
|
||||
}
|
||||
|
@ -214,10 +215,10 @@ void ContactSolver::initializeContactConstraints() {
|
|||
}
|
||||
|
||||
// Get the velocities of the bodies
|
||||
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
|
||||
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
|
||||
|
||||
// For each contact point constraint
|
||||
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||
|
@ -474,10 +475,10 @@ void ContactSolver::solveContactConstraints() {
|
|||
|
||||
decimal sumPenetrationImpulse = 0.0;
|
||||
|
||||
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
|
||||
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
|
||||
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
|
||||
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
|
||||
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
|
||||
|
||||
for (uint i=0; i<contactManifold.nbContacts; i++) {
|
||||
|
||||
|
@ -721,15 +722,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSo
|
|||
|
||||
// Update the velocities of the bodies by applying the impulse P
|
||||
if (contactManifold.isBody1Moving) {
|
||||
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
|
||||
mConstrainedLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
|
||||
impulse.linearImpulseBody1;
|
||||
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
|
||||
mConstrainedAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
|
||||
impulse.angularImpulseBody1;
|
||||
}
|
||||
if (contactManifold.isBody2Moving) {
|
||||
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
|
||||
mConstrainedLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
|
||||
impulse.linearImpulseBody2;
|
||||
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
|
||||
mConstrainedAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
|
||||
impulse.angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
@ -758,10 +759,6 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse,
|
|||
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
||||
ContactPointSolver& contactPoint) const {
|
||||
|
||||
// Update the old friction vectors
|
||||
//contact.oldFrictionVector1 = contact.frictionVector1;
|
||||
//contact.oldFrictionVector2 = contact.frictionVector2;
|
||||
|
||||
assert(contactPoint.normal.length() > 0.0);
|
||||
|
||||
// Compute the velocity difference vector in the tangential plane
|
||||
|
@ -819,19 +816,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
|||
|
||||
// Clean up the constraint solver
|
||||
void ContactSolver::cleanup() {
|
||||
mMapBodyToIndex.clear();
|
||||
//mMapBodyToIndex.clear();
|
||||
mConstraintBodies.clear();
|
||||
|
||||
if (mContactConstraints != 0) {
|
||||
if (mContactConstraints != NULL) {
|
||||
delete[] mContactConstraints;
|
||||
mContactConstraints = 0;
|
||||
mContactConstraints = NULL;
|
||||
}
|
||||
if (mLinearVelocities != 0) {
|
||||
delete[] mLinearVelocities;
|
||||
mLinearVelocities = 0;
|
||||
if (mSplitLinearVelocities != NULL) {
|
||||
delete[] mSplitLinearVelocities;
|
||||
mSplitLinearVelocities = NULL;
|
||||
}
|
||||
if (mAngularVelocities != 0) {
|
||||
delete[] mAngularVelocities;
|
||||
mAngularVelocities = 0;
|
||||
if (mSplitAngularVelocities != NULL) {
|
||||
delete[] mSplitAngularVelocities;
|
||||
mSplitAngularVelocities = NULL;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -62,7 +62,7 @@ struct Impulse {
|
|||
|
||||
/* -------------------------------------------------------------------
|
||||
Class ContactSolver :
|
||||
This class represents the contacts solver that is used is solve rigid bodies contacts.
|
||||
This class represents the contact solver that is used is solve 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).
|
||||
|
||||
|
@ -228,12 +228,6 @@ class ContactSolver {
|
|||
// Number of iterations of the constraints solver
|
||||
uint mNbIterations;
|
||||
|
||||
// Array of constrained linear velocities
|
||||
Vector3* mLinearVelocities;
|
||||
|
||||
// Array of constrained angular velocities
|
||||
Vector3* mAngularVelocities;
|
||||
|
||||
// Split linear velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitLinearVelocities;
|
||||
|
||||
|
@ -252,8 +246,16 @@ class ContactSolver {
|
|||
// Constrained bodies
|
||||
std::set<RigidBody*> mConstraintBodies;
|
||||
|
||||
// Map body to index
|
||||
std::map<RigidBody*, uint> mMapBodyToIndex;
|
||||
// Pointer to the array of constrained linear velocities (state of the linear velocities
|
||||
// after solving the constraints)
|
||||
std::vector<Vector3>& mConstrainedLinearVelocities;
|
||||
|
||||
// Pointer to the array of constrained angular velocities (state of the angular velocities
|
||||
// after solving the constraints)
|
||||
std::vector<Vector3>& mConstrainedAngularVelocities;
|
||||
|
||||
// Reference to the map of rigid body to their index in the constrained velocities array
|
||||
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
||||
|
||||
// True if the warm starting of the solver is active
|
||||
bool mIsWarmStartingActive;
|
||||
|
@ -327,7 +329,9 @@ class ContactSolver {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
// Constructor
|
||||
ContactSolver(DynamicsWorld& mWorld);
|
||||
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
|
||||
std::vector<Vector3>& constrainedAngularVelocities, const std::map<RigidBody*, uint>&
|
||||
mapBodyToVelocityIndex);
|
||||
|
||||
// Destructor
|
||||
virtual ~ContactSolver();
|
||||
|
@ -369,31 +373,17 @@ inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
|
|||
return mConstraintBodies.count(body) == 1;
|
||||
}
|
||||
|
||||
// Return the constrained linear velocity of a body after solving the constraints
|
||||
inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mLinearVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the split linear velocity
|
||||
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||
return mSplitLinearVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the constraints
|
||||
inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mAngularVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the split angular velocity
|
||||
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
|
||||
return mSplitAngularVelocities[indexBody];
|
||||
}
|
||||
|
||||
|
@ -454,6 +444,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
|
|||
contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda);
|
||||
}
|
||||
|
||||
} // End of ReactPhysics3D namespace
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -26,13 +26,15 @@
|
|||
// Libraries
|
||||
#include "DynamicsWorld.h"
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
// Namespaces
|
||||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// Constructor
|
||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mContactSolver(*this),
|
||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
|
||||
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
||||
mMapBodyToConstrainedVelocityIndex),
|
||||
mIsDeactivationActive(DEACTIVATION_ENABLED) {
|
||||
|
||||
}
|
||||
|
@ -46,6 +48,9 @@ DynamicsWorld::~DynamicsWorld() {
|
|||
(*it).second->OverlappingPair::~OverlappingPair();
|
||||
mMemoryPoolOverlappingPairs.freeObject((*it).second);
|
||||
}
|
||||
|
||||
// Free the allocated memory for the constrained velocities
|
||||
cleanupConstrainedVelocitiesArray();
|
||||
}
|
||||
|
||||
// Update the physics simulation
|
||||
|
@ -68,6 +73,9 @@ void DynamicsWorld::update() {
|
|||
// Compute the collision detection
|
||||
mCollisionDetection.computeCollisionDetection();
|
||||
|
||||
// Initialize the constrained velocities
|
||||
initConstrainedVelocitiesArray();
|
||||
|
||||
// If there are contacts
|
||||
if (!mContactManifolds.empty()) {
|
||||
|
||||
|
@ -82,60 +90,61 @@ void DynamicsWorld::update() {
|
|||
resetBodiesMovementVariable();
|
||||
|
||||
// Update the position and orientation of each body
|
||||
updateAllBodiesMotion();
|
||||
updateRigidBodiesPositionAndOrientation();
|
||||
|
||||
// Cleanup of the contact solver
|
||||
mContactSolver.cleanup();
|
||||
|
||||
// Cleanup the constrained velocities
|
||||
cleanupConstrainedVelocitiesArray();
|
||||
}
|
||||
|
||||
// Compute and set the interpolation factor to all the bodies
|
||||
setInterpolationFactorToAllBodies();
|
||||
}
|
||||
|
||||
// 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 DynamicsWorld::updateAllBodiesMotion() {
|
||||
// Update the position and orientation of the rigid bodies
|
||||
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
|
||||
decimal dt = mTimer.getTimeStep();
|
||||
Vector3 newLinearVelocity;
|
||||
Vector3 newAngularVelocity;
|
||||
|
||||
// For each body of thephysics world
|
||||
// For each rigid body of the world
|
||||
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
|
||||
RigidBody* rigidBody = *it;
|
||||
assert(rigidBody);
|
||||
|
||||
// If the body is able to move
|
||||
// If the body is allowed 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 (mContactSolver.isConstrainedBody(*it)) {
|
||||
// Get the constrained linear and angular velocities from the constraint solver
|
||||
newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody);
|
||||
newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
else {
|
||||
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
|
||||
// external forces and torques.
|
||||
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
// Update the old Transform of the body
|
||||
rigidBody->updateOldTransform();
|
||||
|
||||
// Add the velocity V1 to the new velocity
|
||||
newLinearVelocity += rigidBody->getLinearVelocity();
|
||||
newAngularVelocity += rigidBody->getAngularVelocity();
|
||||
// Get the constrained velocity
|
||||
uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
|
||||
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
|
||||
|
||||
// Update the linear and angular velocity of the body
|
||||
rigidBody->setLinearVelocity(newLinVelocity);
|
||||
rigidBody->setAngularVelocity(newAngVelocity);
|
||||
|
||||
// Add the split impulse velocity from Contact Solver (only used to update the position)
|
||||
if (mContactSolver.isConstrainedBody(rigidBody)) {
|
||||
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
|
||||
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
|
||||
// Update the position and the orientation of the body according to the new velocity
|
||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
|
||||
|
||||
// Get current position and orientation of the body
|
||||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
||||
|
||||
// Compute the new position of the body
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
|
||||
|
||||
// Update the Transform of the body
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
rigidBody->setTransform(newTransform);
|
||||
|
||||
// Update the AABB of the rigid body
|
||||
rigidBody->updateAABB();
|
||||
|
@ -143,40 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
}
|
||||
}
|
||||
|
||||
// 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 DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
|
||||
Vector3 newLinVelocity,
|
||||
Vector3 newAngVelocity) {
|
||||
decimal dt = mTimer.getTimeStep();
|
||||
|
||||
assert(rigidBody);
|
||||
|
||||
// Update the old position and orientation of the body
|
||||
rigidBody->updateOldTransform();
|
||||
|
||||
// Update the linear and angular velocity of the body
|
||||
rigidBody->setLinearVelocity(newLinVelocity);
|
||||
rigidBody->setAngularVelocity(newAngVelocity);
|
||||
|
||||
// Split velocity (only used to update the position)
|
||||
if (mContactSolver.isConstrainedBody(rigidBody)) {
|
||||
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
|
||||
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
|
||||
// Get current position and orientation of the body
|
||||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
|
||||
|
||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
||||
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
|
||||
|
||||
Transform newTransform(newPosition, newOrientation.getUnit());
|
||||
rigidBody->setTransform(newTransform);
|
||||
}
|
||||
|
||||
// Compute and set the interpolation factor to all bodies
|
||||
void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
||||
|
||||
|
@ -194,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
|||
}
|
||||
}
|
||||
|
||||
// Initialize the constrained velocities array at each step
|
||||
void DynamicsWorld::initConstrainedVelocitiesArray() {
|
||||
|
||||
// TODO : Use better memory allocation here
|
||||
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
||||
|
||||
double dt = mTimer.getTimeStep();
|
||||
|
||||
// Fill in the mapping of rigid body to their index in the constrained
|
||||
// velocities arrays
|
||||
uint i = 0;
|
||||
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
RigidBody* rigidBody = *it;
|
||||
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
|
||||
|
||||
// TODO : Move it somewhere else
|
||||
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
|
||||
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
// Cleanup the constrained velocities array at each step
|
||||
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
|
||||
|
||||
// Clear the constrained velocites
|
||||
mConstrainedLinearVelocities.clear();
|
||||
mConstrainedAngularVelocities.clear();
|
||||
|
||||
// Clear the rigid body to velocities array index mapping
|
||||
mMapBodyToConstrainedVelocityIndex.clear();
|
||||
}
|
||||
|
||||
// Apply the gravity force to all bodies of the physics world
|
||||
void DynamicsWorld::applyGravity() {
|
||||
|
||||
|
|
|
@ -84,6 +84,17 @@ class DynamicsWorld : public CollisionWorld {
|
|||
// Memory pool for the contacts
|
||||
MemoryPool<Contact> mMemoryPoolContacts;
|
||||
|
||||
// Array of constrained linear velocities (state of the linear velocities
|
||||
// after solving the constraints)
|
||||
std::vector<Vector3> mConstrainedLinearVelocities;
|
||||
|
||||
// Array of constrained angular velocities (state of the angular velocities
|
||||
// after solving the constraints)
|
||||
std::vector<Vector3> mConstrainedAngularVelocities;
|
||||
|
||||
// Map body to their index in the constrained velocities array
|
||||
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
// Private copy-constructor
|
||||
|
@ -93,7 +104,7 @@ class DynamicsWorld : public CollisionWorld {
|
|||
DynamicsWorld& operator=(const DynamicsWorld& world);
|
||||
|
||||
// Compute the motion of all bodies and update their positions and orientations
|
||||
void updateAllBodiesMotion();
|
||||
void updateRigidBodiesPositionAndOrientation();
|
||||
|
||||
// Update the position and orientation of a body
|
||||
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||
|
@ -102,6 +113,12 @@ class DynamicsWorld : public CollisionWorld {
|
|||
// Compute and set the interpolation factor to all bodies
|
||||
void setInterpolationFactorToAllBodies();
|
||||
|
||||
// Initialize the constrained velocities array at each step
|
||||
void initConstrainedVelocitiesArray();
|
||||
|
||||
// Cleanup the constrained velocities array at each step
|
||||
void cleanupConstrainedVelocitiesArray();
|
||||
|
||||
// Apply the gravity force to all bodies
|
||||
void applyGravity();
|
||||
|
||||
|
@ -169,6 +186,9 @@ public :
|
|||
// Set the isGravityOn attribute
|
||||
void setIsGratityOn(bool isGravityOn);
|
||||
|
||||
// Return the number of rigid bodies in the world
|
||||
uint getNbRigidBodies() const;
|
||||
|
||||
// Add a constraint
|
||||
void addConstraint(Constraint* constraint);
|
||||
|
||||
|
@ -286,6 +306,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
|
|||
mIsGravityOn = isGravityOn;
|
||||
}
|
||||
|
||||
// Return the number of rigid bodies in the world
|
||||
inline uint DynamicsWorld::getNbRigidBodies() const {
|
||||
return mRigidBodies.size();
|
||||
}
|
||||
|
||||
// Return an iterator to the beginning of the bodies of the physics world
|
||||
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
|
||||
return mRigidBodies.begin();
|
||||
|
|
Loading…
Reference in New Issue
Block a user