The time accumulator and frame interpolation should now be done outside the physics engine
This commit is contained in:
parent
a1eade1c02
commit
fb90eac003
|
@ -117,8 +117,6 @@ SET (REACTPHYSICS3D_SOURCES
|
||||||
"src/engine/OverlappingPair.cpp"
|
"src/engine/OverlappingPair.cpp"
|
||||||
"src/engine/Profiler.h"
|
"src/engine/Profiler.h"
|
||||||
"src/engine/Profiler.cpp"
|
"src/engine/Profiler.cpp"
|
||||||
"src/engine/Timer.h"
|
|
||||||
"src/engine/Timer.cpp"
|
|
||||||
"src/mathematics/mathematics.h"
|
"src/mathematics/mathematics.h"
|
||||||
"src/mathematics/mathematics_functions.h"
|
"src/mathematics/mathematics_functions.h"
|
||||||
"src/mathematics/Matrix2x2.h"
|
"src/mathematics/Matrix2x2.h"
|
||||||
|
|
|
@ -41,8 +41,6 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world,
|
||||||
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
|
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
|
||||||
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
|
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
|
||||||
|
|
||||||
mInterpolationFactor = 0.0;
|
|
||||||
|
|
||||||
// Initialize the old transform
|
// Initialize the old transform
|
||||||
mOldTransform = transform;
|
mOldTransform = transform;
|
||||||
}
|
}
|
||||||
|
|
|
@ -76,9 +76,6 @@ class CollisionBody : public Body {
|
||||||
/// Last position and orientation of the body
|
/// Last position and orientation of the body
|
||||||
Transform mOldTransform;
|
Transform mOldTransform;
|
||||||
|
|
||||||
/// Interpolation factor used for the state interpolation
|
|
||||||
decimal mInterpolationFactor;
|
|
||||||
|
|
||||||
/// First element of the linked list of proxy collision shapes of this body
|
/// First element of the linked list of proxy collision shapes of this body
|
||||||
ProxyShape* mProxyCollisionShapes;
|
ProxyShape* mProxyCollisionShapes;
|
||||||
|
|
||||||
|
@ -118,9 +115,6 @@ class CollisionBody : public Body {
|
||||||
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
|
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
|
||||||
int resetIsAlreadyInIslandAndCountManifolds();
|
int resetIsAlreadyInIslandAndCountManifolds();
|
||||||
|
|
||||||
/// Set the interpolation factor of the body
|
|
||||||
void setInterpolationFactor(decimal factor);
|
|
||||||
|
|
||||||
public :
|
public :
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -153,9 +147,6 @@ class CollisionBody : public Body {
|
||||||
/// Remove a collision shape from the body
|
/// Remove a collision shape from the body
|
||||||
virtual void removeCollisionShape(const ProxyShape* proxyShape);
|
virtual void removeCollisionShape(const ProxyShape* proxyShape);
|
||||||
|
|
||||||
/// Return the interpolated transform for rendering
|
|
||||||
Transform getInterpolatedTransform() const;
|
|
||||||
|
|
||||||
/// Return the first element of the linked list of contact manifolds involving this body
|
/// Return the first element of the linked list of contact manifolds involving this body
|
||||||
const ContactManifoldListElement* getContactManifoldsList() const;
|
const ContactManifoldListElement* getContactManifoldsList() const;
|
||||||
|
|
||||||
|
@ -227,20 +218,6 @@ inline void CollisionBody::setType(BodyType type) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the interpolated transform for rendering
|
|
||||||
/**
|
|
||||||
* @return The current interpolated transformation (between previous and current frame)
|
|
||||||
*/
|
|
||||||
inline Transform CollisionBody::getInterpolatedTransform() const {
|
|
||||||
return Transform::interpolateTransforms(mOldTransform, mTransform, mInterpolationFactor);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the interpolation factor of the body
|
|
||||||
inline void CollisionBody::setInterpolationFactor(decimal factor) {
|
|
||||||
// Set the factor
|
|
||||||
mInterpolationFactor = factor;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the current position and orientation
|
// Return the current position and orientation
|
||||||
/**
|
/**
|
||||||
* @return The current transformation of the body that transforms the local-space
|
* @return The current transformation of the body that transforms the local-space
|
||||||
|
|
|
@ -343,7 +343,7 @@ void RigidBody::updateBroadPhaseState() const {
|
||||||
PROFILE("RigidBody::updateBroadPhaseState()");
|
PROFILE("RigidBody::updateBroadPhaseState()");
|
||||||
|
|
||||||
DynamicsWorld& world = dynamic_cast<DynamicsWorld&>(mWorld);
|
DynamicsWorld& world = dynamic_cast<DynamicsWorld&>(mWorld);
|
||||||
const Vector3 displacement = world.mTimer.getTimeStep() * mLinearVelocity;
|
const Vector3 displacement = world.mTimeStep* mLinearVelocity;
|
||||||
|
|
||||||
// For all the proxy collision shapes of the body
|
// For all the proxy collision shapes of the body
|
||||||
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
|
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
|
||||||
|
|
|
@ -83,9 +83,6 @@ const decimal PI = decimal(3.14159265);
|
||||||
/// 2*Pi constant
|
/// 2*Pi constant
|
||||||
const decimal PI_TIMES_2 = decimal(6.28318530);
|
const decimal PI_TIMES_2 = decimal(6.28318530);
|
||||||
|
|
||||||
/// Default internal constant timestep in seconds
|
|
||||||
const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0);
|
|
||||||
|
|
||||||
/// Default friction coefficient for a rigid body
|
/// Default friction coefficient for a rigid body
|
||||||
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
|
const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
|
||||||
|
|
||||||
|
|
|
@ -37,10 +37,9 @@ using namespace std;
|
||||||
// Constructor
|
// Constructor
|
||||||
/**
|
/**
|
||||||
* @param gravity Gravity vector in the world (in meters per second squared)
|
* @param gravity Gravity vector in the world (in meters per second squared)
|
||||||
* @param timeStep Time step for an internal physics tick (in seconds)
|
|
||||||
*/
|
*/
|
||||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
|
||||||
: CollisionWorld(), mTimer(timeStep),
|
: CollisionWorld(),
|
||||||
mContactSolver(mMapBodyToConstrainedVelocityIndex),
|
mContactSolver(mMapBodyToConstrainedVelocityIndex),
|
||||||
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
|
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
|
||||||
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
||||||
|
@ -95,7 +94,10 @@ DynamicsWorld::~DynamicsWorld() {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update the physics simulation
|
// Update the physics simulation
|
||||||
void DynamicsWorld::update() {
|
/**
|
||||||
|
* @param timeStep The amount of time to step the simulation by (in seconds)
|
||||||
|
*/
|
||||||
|
void DynamicsWorld::update(decimal timeStep) {
|
||||||
|
|
||||||
#ifdef IS_PROFILING_ACTIVE
|
#ifdef IS_PROFILING_ACTIVE
|
||||||
// Increment the frame counter of the profiler
|
// Increment the frame counter of the profiler
|
||||||
|
@ -104,49 +106,39 @@ void DynamicsWorld::update() {
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::update()");
|
PROFILE("DynamicsWorld::update()");
|
||||||
|
|
||||||
assert(mTimer.getIsRunning());
|
mTimeStep = timeStep;
|
||||||
|
|
||||||
// Compute the time since the last update() call and update the timer
|
|
||||||
mTimer.update();
|
|
||||||
|
|
||||||
// While the time accumulator is not empty
|
// Notify the event listener about the beginning of an internal tick
|
||||||
while(mTimer.isPossibleToTakeStep()) {
|
if (mEventListener != NULL) mEventListener->beginInternalTick();
|
||||||
|
|
||||||
// Notify the event listener about the beginning of an internal tick
|
// Reset all the contact manifolds lists of each body
|
||||||
if (mEventListener != NULL) mEventListener->beginInternalTick();
|
resetContactManifoldListsOfBodies();
|
||||||
|
|
||||||
// Reset all the contact manifolds lists of each body
|
// Compute the collision detection
|
||||||
resetContactManifoldListsOfBodies();
|
mCollisionDetection.computeCollisionDetection();
|
||||||
|
|
||||||
// Compute the collision detection
|
|
||||||
mCollisionDetection.computeCollisionDetection();
|
|
||||||
|
|
||||||
// Compute the islands (separate groups of bodies with constraints between each others)
|
// Compute the islands (separate groups of bodies with constraints between each others)
|
||||||
computeIslands();
|
computeIslands();
|
||||||
|
|
||||||
// Integrate the velocities
|
// Integrate the velocities
|
||||||
integrateRigidBodiesVelocities();
|
integrateRigidBodiesVelocities();
|
||||||
|
|
||||||
// Update the timer
|
// Solve the contacts and constraints
|
||||||
mTimer.nextStep();
|
solveContactsAndConstraints();
|
||||||
|
|
||||||
// Solve the contacts and constraints
|
// Integrate the position and orientation of each body
|
||||||
solveContactsAndConstraints();
|
integrateRigidBodiesPositions();
|
||||||
|
|
||||||
// Integrate the position and orientation of each body
|
// Solve the position correction for constraints
|
||||||
integrateRigidBodiesPositions();
|
solvePositionCorrection();
|
||||||
|
|
||||||
// Solve the position correction for constraints
|
// Update the state (positions and velocities) of the bodies
|
||||||
solvePositionCorrection();
|
updateBodiesState();
|
||||||
|
|
||||||
// Update the state (positions and velocities) of the bodies
|
if (mIsSleepingEnabled) updateSleepingBodies();
|
||||||
updateBodiesState();
|
|
||||||
|
|
||||||
if (mIsSleepingEnabled) updateSleepingBodies();
|
// Notify the event listener about the end of an internal tick
|
||||||
|
if (mEventListener != NULL) mEventListener->endInternalTick();
|
||||||
// Notify the event listener about the end of an internal tick
|
|
||||||
if (mEventListener != NULL) mEventListener->endInternalTick();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Reset the external force and torque applied to the bodies
|
// Reset the external force and torque applied to the bodies
|
||||||
resetBodiesForceAndTorque();
|
resetBodiesForceAndTorque();
|
||||||
|
@ -161,8 +153,6 @@ void DynamicsWorld::update() {
|
||||||
void DynamicsWorld::integrateRigidBodiesPositions() {
|
void DynamicsWorld::integrateRigidBodiesPositions() {
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
|
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
|
||||||
|
|
||||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
|
||||||
|
|
||||||
// For each island of the world
|
// For each island of the world
|
||||||
for (uint i=0; i < mNbIslands; i++) {
|
for (uint i=0; i < mNbIslands; i++) {
|
||||||
|
@ -190,10 +180,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
||||||
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
|
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
|
||||||
|
|
||||||
// Update the new constrained position and orientation of the body
|
// Update the new constrained position and orientation of the body
|
||||||
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
|
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
|
||||||
mConstrainedOrientations[indexArray] = currentOrientation +
|
mConstrainedOrientations[indexArray] = currentOrientation +
|
||||||
Quaternion(0, newAngVelocity) *
|
Quaternion(0, newAngVelocity) *
|
||||||
currentOrientation * decimal(0.5) * dt;
|
currentOrientation * decimal(0.5) * mTimeStep;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -232,23 +222,6 @@ void DynamicsWorld::updateBodiesState() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute and set the interpolation factor to all bodies
|
|
||||||
void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::setInterpolationFactorToAllBodies()");
|
|
||||||
|
|
||||||
// Compute the interpolation factor
|
|
||||||
decimal factor = mTimer.computeInterpolationFactor();
|
|
||||||
assert(factor >= 0.0 && factor <= 1.0);
|
|
||||||
|
|
||||||
// Set the factor to all bodies
|
|
||||||
set<RigidBody*>::iterator it;
|
|
||||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
|
||||||
|
|
||||||
(*it)->setInterpolationFactor(factor);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Initialize the bodies velocities arrays for the next simulation step.
|
// Initialize the bodies velocities arrays for the next simulation step.
|
||||||
void DynamicsWorld::initVelocityArrays() {
|
void DynamicsWorld::initVelocityArrays() {
|
||||||
|
|
||||||
|
@ -305,8 +278,6 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
||||||
// Initialize the bodies velocity arrays
|
// Initialize the bodies velocity arrays
|
||||||
initVelocityArrays();
|
initVelocityArrays();
|
||||||
|
|
||||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
|
||||||
|
|
||||||
// For each island of the world
|
// For each island of the world
|
||||||
for (uint i=0; i < mNbIslands; i++) {
|
for (uint i=0; i < mNbIslands; i++) {
|
||||||
|
|
||||||
|
@ -323,16 +294,16 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
||||||
|
|
||||||
// Integrate the external force to get the new velocity of the body
|
// Integrate the external force to get the new velocity of the body
|
||||||
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
|
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
|
||||||
dt * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
|
mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
|
||||||
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
|
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
|
||||||
dt * bodies[b]->getInertiaTensorInverseWorld() *
|
mTimeStep * bodies[b]->getInertiaTensorInverseWorld() *
|
||||||
bodies[b]->mExternalTorque;
|
bodies[b]->mExternalTorque;
|
||||||
|
|
||||||
// If the gravity has to be applied to this rigid body
|
// If the gravity has to be applied to this rigid body
|
||||||
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
|
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
|
||||||
|
|
||||||
// Integrate the gravity force
|
// Integrate the gravity force
|
||||||
mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->mMassInverse *
|
mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse *
|
||||||
bodies[b]->getMass() * mGravity;
|
bodies[b]->getMass() * mGravity;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -351,9 +322,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
||||||
// => v2 = v1 * (1 - c * dt)
|
// => v2 = v1 * (1 - c * dt)
|
||||||
decimal linDampingFactor = bodies[b]->getLinearDamping();
|
decimal linDampingFactor = bodies[b]->getLinearDamping();
|
||||||
decimal angDampingFactor = bodies[b]->getAngularDamping();
|
decimal angDampingFactor = bodies[b]->getAngularDamping();
|
||||||
decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
|
decimal linearDamping = clamp(decimal(1.0) - mTimeStep * linDampingFactor,
|
||||||
decimal(0.0), decimal(1.0));
|
decimal(0.0), decimal(1.0));
|
||||||
decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
|
decimal angularDamping = clamp(decimal(1.0) - mTimeStep * angDampingFactor,
|
||||||
decimal(0.0), decimal(1.0));
|
decimal(0.0), decimal(1.0));
|
||||||
mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
|
mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
|
||||||
decimal(1.0));
|
decimal(1.0));
|
||||||
|
@ -373,9 +344,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
|
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
|
||||||
|
|
||||||
// Get the current time step
|
|
||||||
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
|
||||||
|
|
||||||
// Set the velocities arrays
|
// Set the velocities arrays
|
||||||
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
|
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
|
||||||
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
||||||
|
@ -399,7 +367,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
||||||
if (isContactsToSolve) {
|
if (isContactsToSolve) {
|
||||||
|
|
||||||
// Initialize the solver
|
// Initialize the solver
|
||||||
mContactSolver.initializeForIsland(dt, mIslands[islandIndex]);
|
mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
|
||||||
|
|
||||||
// Warm start the contact solver
|
// Warm start the contact solver
|
||||||
mContactSolver.warmStart();
|
mContactSolver.warmStart();
|
||||||
|
@ -409,7 +377,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
||||||
if (isConstraintsToSolve) {
|
if (isConstraintsToSolve) {
|
||||||
|
|
||||||
// Initialize the constraint solver
|
// Initialize the constraint solver
|
||||||
mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]);
|
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// For each iteration of the velocity solver
|
// For each iteration of the velocity solver
|
||||||
|
@ -811,7 +779,6 @@ void DynamicsWorld::updateSleepingBodies() {
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::updateSleepingBodies()");
|
PROFILE("DynamicsWorld::updateSleepingBodies()");
|
||||||
|
|
||||||
const decimal dt = static_cast<decimal>(mTimer.getTimeStep());
|
|
||||||
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
|
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
|
||||||
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
|
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
|
||||||
|
|
||||||
|
@ -839,7 +806,7 @@ void DynamicsWorld::updateSleepingBodies() {
|
||||||
else { // If the body velocity is bellow the sleeping velocity threshold
|
else { // If the body velocity is bellow the sleeping velocity threshold
|
||||||
|
|
||||||
// Increase the sleep time
|
// Increase the sleep time
|
||||||
bodies[b]->mSleepTime += dt;
|
bodies[b]->mSleepTime += mTimeStep;
|
||||||
if (bodies[b]->mSleepTime < minSleepTime) {
|
if (bodies[b]->mSleepTime < minSleepTime) {
|
||||||
minSleepTime = bodies[b]->mSleepTime;
|
minSleepTime = bodies[b]->mSleepTime;
|
||||||
}
|
}
|
||||||
|
|
|
@ -32,7 +32,6 @@
|
||||||
#include "ContactSolver.h"
|
#include "ContactSolver.h"
|
||||||
#include "ConstraintSolver.h"
|
#include "ConstraintSolver.h"
|
||||||
#include "body/RigidBody.h"
|
#include "body/RigidBody.h"
|
||||||
#include "Timer.h"
|
|
||||||
#include "Island.h"
|
#include "Island.h"
|
||||||
#include "configuration.h"
|
#include "configuration.h"
|
||||||
|
|
||||||
|
@ -51,9 +50,6 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Timer of the physics engine
|
|
||||||
Timer mTimer;
|
|
||||||
|
|
||||||
/// Contact solver
|
/// Contact solver
|
||||||
ContactSolver mContactSolver;
|
ContactSolver mContactSolver;
|
||||||
|
|
||||||
|
@ -78,6 +74,9 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
/// Gravity vector of the world
|
/// Gravity vector of the world
|
||||||
Vector3 mGravity;
|
Vector3 mGravity;
|
||||||
|
|
||||||
|
/// Current frame time step (in seconds)
|
||||||
|
decimal mTimeStep;
|
||||||
|
|
||||||
/// True if the gravity force is on
|
/// True if the gravity force is on
|
||||||
bool mIsGravityEnabled;
|
bool mIsGravityEnabled;
|
||||||
|
|
||||||
|
@ -182,7 +181,7 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
DynamicsWorld(const Vector3& mGravity, decimal timeStep);
|
DynamicsWorld(const Vector3& mGravity);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~DynamicsWorld();
|
virtual ~DynamicsWorld();
|
||||||
|
@ -194,7 +193,7 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
void stop();
|
void stop();
|
||||||
|
|
||||||
/// Update the physics simulation
|
/// Update the physics simulation
|
||||||
void update();
|
void update(decimal timeStep);
|
||||||
|
|
||||||
/// Set the number of iterations for the velocity constraint solver
|
/// Set the number of iterations for the velocity constraint solver
|
||||||
void setNbIterationsVelocitySolver(uint nbIterations);
|
void setNbIterationsVelocitySolver(uint nbIterations);
|
||||||
|
@ -312,16 +311,7 @@ inline void DynamicsWorld::resetBodiesForceAndTorque() {
|
||||||
(*it)->mExternalForce.setToZero();
|
(*it)->mExternalForce.setToZero();
|
||||||
(*it)->mExternalTorque.setToZero();
|
(*it)->mExternalTorque.setToZero();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Start the physics simulation
|
|
||||||
inline void DynamicsWorld::start() {
|
|
||||||
mTimer.start();
|
|
||||||
}
|
|
||||||
|
|
||||||
inline void DynamicsWorld::stop() {
|
|
||||||
mTimer.stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the number of iterations for the velocity constraint solver
|
// Set the number of iterations for the velocity constraint solver
|
||||||
/**
|
/**
|
||||||
|
@ -434,14 +424,6 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
|
||||||
return mRigidBodies.end();
|
return mRigidBodies.end();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the current physics time (in seconds)
|
|
||||||
/**
|
|
||||||
* @return The current physics time (in seconds)
|
|
||||||
*/
|
|
||||||
inline long double DynamicsWorld::getPhysicsTime() const {
|
|
||||||
return mTimer.getPhysicsTime();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return true if the sleeping technique is enabled
|
// Return true if the sleeping technique is enabled
|
||||||
/**
|
/**
|
||||||
* @return True if the sleeping technique is enabled and false otherwise
|
* @return True if the sleeping technique is enabled and false otherwise
|
||||||
|
|
|
@ -1,66 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
|
|
||||||
* Copyright (c) 2010-2015 Daniel Chappuis *
|
|
||||||
*********************************************************************************
|
|
||||||
* *
|
|
||||||
* This software is provided 'as-is', without any express or implied warranty. *
|
|
||||||
* In no event will the authors be held liable for any damages arising from the *
|
|
||||||
* use of this software. *
|
|
||||||
* *
|
|
||||||
* Permission is granted to anyone to use this software for any purpose, *
|
|
||||||
* including commercial applications, and to alter it and redistribute it *
|
|
||||||
* freely, subject to the following restrictions: *
|
|
||||||
* *
|
|
||||||
* 1. The origin of this software must not be misrepresented; you must not claim *
|
|
||||||
* that you wrote the original software. If you use this software in a *
|
|
||||||
* product, an acknowledgment in the product documentation would be *
|
|
||||||
* appreciated but is not required. *
|
|
||||||
* *
|
|
||||||
* 2. Altered source versions must be plainly marked as such, and must not be *
|
|
||||||
* misrepresented as being the original software. *
|
|
||||||
* *
|
|
||||||
* 3. This notice may not be removed or altered from any source distribution. *
|
|
||||||
* *
|
|
||||||
********************************************************************************/
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include "Timer.h"
|
|
||||||
|
|
||||||
// We want to use the ReactPhysics3D namespace
|
|
||||||
using namespace reactphysics3d;
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
Timer::Timer(double timeStep) : mTimeStep(timeStep), mIsRunning(false) {
|
|
||||||
assert(timeStep > 0.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
Timer::~Timer() {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the current time of the system in seconds
|
|
||||||
long double Timer::getCurrentSystemTime() {
|
|
||||||
|
|
||||||
#if defined(WINDOWS_OS)
|
|
||||||
LARGE_INTEGER ticksPerSecond;
|
|
||||||
LARGE_INTEGER ticks;
|
|
||||||
QueryPerformanceFrequency(&ticksPerSecond);
|
|
||||||
QueryPerformanceCounter(&ticks);
|
|
||||||
return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart));
|
|
||||||
#else
|
|
||||||
// Initialize the lastUpdateTime with the current time in seconds
|
|
||||||
timeval timeValue;
|
|
||||||
gettimeofday(&timeValue, NULL);
|
|
||||||
return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0));
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,200 +0,0 @@
|
||||||
/********************************************************************************
|
|
||||||
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
|
|
||||||
* Copyright (c) 2010-2015 Daniel Chappuis *
|
|
||||||
*********************************************************************************
|
|
||||||
* *
|
|
||||||
* This software is provided 'as-is', without any express or implied warranty. *
|
|
||||||
* In no event will the authors be held liable for any damages arising from the *
|
|
||||||
* use of this software. *
|
|
||||||
* *
|
|
||||||
* Permission is granted to anyone to use this software for any purpose, *
|
|
||||||
* including commercial applications, and to alter it and redistribute it *
|
|
||||||
* freely, subject to the following restrictions: *
|
|
||||||
* *
|
|
||||||
* 1. The origin of this software must not be misrepresented; you must not claim *
|
|
||||||
* that you wrote the original software. If you use this software in a *
|
|
||||||
* product, an acknowledgment in the product documentation would be *
|
|
||||||
* appreciated but is not required. *
|
|
||||||
* *
|
|
||||||
* 2. Altered source versions must be plainly marked as such, and must not be *
|
|
||||||
* misrepresented as being the original software. *
|
|
||||||
* *
|
|
||||||
* 3. This notice may not be removed or altered from any source distribution. *
|
|
||||||
* *
|
|
||||||
********************************************************************************/
|
|
||||||
|
|
||||||
#ifndef REACTPHYSICS3D_TIMER_H
|
|
||||||
#define REACTPHYSICS3D_TIMER_H
|
|
||||||
|
|
||||||
// Libraries
|
|
||||||
#include <stdexcept>
|
|
||||||
#include <iostream>
|
|
||||||
#include <ctime>
|
|
||||||
#include <cassert>
|
|
||||||
#include "configuration.h"
|
|
||||||
|
|
||||||
#if defined(WINDOWS_OS) // For Windows platform
|
|
||||||
#define NOMINMAX // This is used to avoid definition of max() and min() macros
|
|
||||||
#include <windows.h>
|
|
||||||
#else // For Mac OS or Linux platform
|
|
||||||
#include <sys/time.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
/// Namespace ReactPhysics3D
|
|
||||||
namespace reactphysics3d {
|
|
||||||
|
|
||||||
// Class Timer
|
|
||||||
/**
|
|
||||||
* This class will take care of the time in the physics engine. It
|
|
||||||
* uses functions that depend on the current platform to get the
|
|
||||||
* current time.
|
|
||||||
*/
|
|
||||||
class Timer {
|
|
||||||
|
|
||||||
private :
|
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
|
||||||
|
|
||||||
/// Timestep dt of the physics engine (timestep > 0.0)
|
|
||||||
double mTimeStep;
|
|
||||||
|
|
||||||
/// Last time the timer has been updated
|
|
||||||
long double mLastUpdateTime;
|
|
||||||
|
|
||||||
/// Time difference between the two last timer update() calls
|
|
||||||
long double mDeltaTime;
|
|
||||||
|
|
||||||
/// Used to fix the time step and avoid strange time effects
|
|
||||||
double mAccumulator;
|
|
||||||
|
|
||||||
/// True if the timer is running
|
|
||||||
bool mIsRunning;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
|
||||||
|
|
||||||
/// Private copy-constructor
|
|
||||||
Timer(const Timer& timer);
|
|
||||||
|
|
||||||
/// Private assignment operator
|
|
||||||
Timer& operator=(const Timer& timer);
|
|
||||||
|
|
||||||
public :
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
|
||||||
|
|
||||||
/// Constructor
|
|
||||||
Timer(double timeStep);
|
|
||||||
|
|
||||||
/// Destructor
|
|
||||||
virtual ~Timer();
|
|
||||||
|
|
||||||
/// Return the timestep of the physics engine
|
|
||||||
double getTimeStep() const;
|
|
||||||
|
|
||||||
/// Set the timestep of the physics engine
|
|
||||||
void setTimeStep(double timeStep);
|
|
||||||
|
|
||||||
/// Return the current time of the physics engine
|
|
||||||
long double getPhysicsTime() const;
|
|
||||||
|
|
||||||
/// Start the timer
|
|
||||||
void start();
|
|
||||||
|
|
||||||
/// Stop the timer
|
|
||||||
void stop();
|
|
||||||
|
|
||||||
/// Return true if the timer is running
|
|
||||||
bool getIsRunning() const;
|
|
||||||
|
|
||||||
/// True if it's possible to take a new step
|
|
||||||
bool isPossibleToTakeStep() const;
|
|
||||||
|
|
||||||
/// Compute the time since the last update() call and add it to the accumulator
|
|
||||||
void update();
|
|
||||||
|
|
||||||
/// Take a new step => update the timer by adding the timeStep value to the current time
|
|
||||||
void nextStep();
|
|
||||||
|
|
||||||
/// Compute the interpolation factor
|
|
||||||
decimal computeInterpolationFactor();
|
|
||||||
|
|
||||||
/// Return the current time of the system in seconds
|
|
||||||
static long double getCurrentSystemTime();
|
|
||||||
};
|
|
||||||
|
|
||||||
// Return the timestep of the physics engine
|
|
||||||
inline double Timer::getTimeStep() const {
|
|
||||||
return mTimeStep;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the timestep of the physics engine
|
|
||||||
inline void Timer::setTimeStep(double timeStep) {
|
|
||||||
assert(timeStep > 0.0f);
|
|
||||||
mTimeStep = timeStep;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return the current time
|
|
||||||
inline long double Timer::getPhysicsTime() const {
|
|
||||||
return mLastUpdateTime;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return if the timer is running
|
|
||||||
inline bool Timer::getIsRunning() const {
|
|
||||||
return mIsRunning;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Start the timer
|
|
||||||
inline void Timer::start() {
|
|
||||||
if (!mIsRunning) {
|
|
||||||
|
|
||||||
// Get the current system time
|
|
||||||
mLastUpdateTime = getCurrentSystemTime();
|
|
||||||
|
|
||||||
mAccumulator = 0.0;
|
|
||||||
mIsRunning = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Stop the timer
|
|
||||||
inline void Timer::stop() {
|
|
||||||
mIsRunning = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// True if it's possible to take a new step
|
|
||||||
inline bool Timer::isPossibleToTakeStep() const {
|
|
||||||
return (mAccumulator >= mTimeStep);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Take a new step => update the timer by adding the timeStep value to the current time
|
|
||||||
inline void Timer::nextStep() {
|
|
||||||
assert(mIsRunning);
|
|
||||||
|
|
||||||
// Update the accumulator value
|
|
||||||
mAccumulator -= mTimeStep;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the interpolation factor
|
|
||||||
inline decimal Timer::computeInterpolationFactor() {
|
|
||||||
return (decimal(mAccumulator / mTimeStep));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the time since the last update() call and add it to the accumulator
|
|
||||||
inline void Timer::update() {
|
|
||||||
|
|
||||||
// Get the current system time
|
|
||||||
long double currentTime = getCurrentSystemTime();
|
|
||||||
|
|
||||||
// Compute the delta display time between two display frames
|
|
||||||
mDeltaTime = currentTime - mLastUpdateTime;
|
|
||||||
|
|
||||||
// Update the current display time
|
|
||||||
mLastUpdateTime = currentTime;
|
|
||||||
|
|
||||||
// Update the accumulator value
|
|
||||||
mAccumulator += mDeltaTime;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
Loading…
Reference in New Issue
Block a user