diff --git a/CMakeLists.txt b/CMakeLists.txt index 819b99ee..38216809 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,8 +117,6 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/OverlappingPair.cpp" "src/engine/Profiler.h" "src/engine/Profiler.cpp" - "src/engine/Timer.h" - "src/engine/Timer.cpp" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" "src/mathematics/Matrix2x2.h" diff --git a/README.md b/README.md index 23d4467b..ca153b31 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ You can find the User Manual and the Doxygen API Documentation [here](http://www ## Branches -The "master" branch always contains the last released version of the library. This is the most stable version. On the other side, +The "master" branch always contains the last released version of the library and some possible bug fixes. This is the most stable version. On the other side, the "develop" branch is used for development. This branch is frequently updated and can be quite unstable. Therefore, if you want to use the library in your application, it is recommended to checkout the "master" branch. diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 0c782d31..f9a4702d 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -41,8 +41,6 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, : Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL), mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) { - mInterpolationFactor = 0.0; - // Initialize the old transform mOldTransform = transform; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index c0d5547b..c75631a5 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -76,9 +76,6 @@ class CollisionBody : public Body { /// Last position and orientation of the body Transform mOldTransform; - /// Interpolation factor used for the state interpolation - decimal mInterpolationFactor; - /// First element of the linked list of proxy collision shapes of this body ProxyShape* mProxyCollisionShapes; @@ -118,9 +115,6 @@ class CollisionBody : public Body { /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds int resetIsAlreadyInIslandAndCountManifolds(); - /// Set the interpolation factor of the body - void setInterpolationFactor(decimal factor); - public : // -------------------- Methods -------------------- // @@ -153,9 +147,6 @@ class CollisionBody : public Body { /// Remove a collision shape from the body 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 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 transformation of the body that transforms the local-space diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 3ba985b6..ddf1ea83 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -343,7 +343,7 @@ void RigidBody::updateBroadPhaseState() const { PROFILE("RigidBody::updateBroadPhaseState()"); DynamicsWorld& world = dynamic_cast(mWorld); - const Vector3 displacement = world.mTimer.getTimeStep() * mLinearVelocity; + const Vector3 displacement = world.mTimeStep* mLinearVelocity; // For all the proxy collision shapes of the body for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 072764b3..3f0b366e 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -224,12 +224,12 @@ inline void ConvexMeshShape::addEdge(uint v1, uint v2) { // If the entry for vertex v1 does not exist in the adjacency list if (mEdgesAdjacencyList.count(v1) == 0) { - mEdgesAdjacencyList.insert(std::make_pair >(v1, std::set())); + mEdgesAdjacencyList.insert(std::make_pair(v1, std::set())); } // If the entry for vertex v2 does not exist in the adjacency list if (mEdgesAdjacencyList.count(v2) == 0) { - mEdgesAdjacencyList.insert(std::make_pair >(v2, std::set())); + mEdgesAdjacencyList.insert(std::make_pair(v2, std::set())); } // Add the edge in the adjacency list diff --git a/src/configuration.h b/src/configuration.h index b4c91f62..50d1872c 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -83,9 +83,6 @@ const decimal PI = decimal(3.14159265); /// 2*Pi constant 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 const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 8c4e2a34..927b28ee 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -37,10 +37,9 @@ using namespace std; // Constructor /** * @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) - : CollisionWorld(), mTimer(timeStep), +DynamicsWorld::DynamicsWorld(const Vector3 &gravity) + : CollisionWorld(), mContactSolver(mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), @@ -95,7 +94,10 @@ DynamicsWorld::~DynamicsWorld() { } // 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 // Increment the frame counter of the profiler @@ -104,49 +106,39 @@ void DynamicsWorld::update() { PROFILE("DynamicsWorld::update()"); - assert(mTimer.getIsRunning()); - - // Compute the time since the last update() call and update the timer - mTimer.update(); + mTimeStep = timeStep; - // While the time accumulator is not empty - while(mTimer.isPossibleToTakeStep()) { + // Notify the event listener about the beginning of an internal tick + if (mEventListener != NULL) mEventListener->beginInternalTick(); - // Notify the event listener about the beginning of an internal tick - if (mEventListener != NULL) mEventListener->beginInternalTick(); + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); - // Reset all the contact manifolds lists of each body - resetContactManifoldListsOfBodies(); - - // Compute the collision detection - mCollisionDetection.computeCollisionDetection(); + // Compute the collision detection + mCollisionDetection.computeCollisionDetection(); - // Compute the islands (separate groups of bodies with constraints between each others) - computeIslands(); + // Compute the islands (separate groups of bodies with constraints between each others) + computeIslands(); - // Integrate the velocities - integrateRigidBodiesVelocities(); + // Integrate the velocities + integrateRigidBodiesVelocities(); - // Update the timer - mTimer.nextStep(); + // Solve the contacts and constraints + solveContactsAndConstraints(); - // Solve the contacts and constraints - solveContactsAndConstraints(); + // Integrate the position and orientation of each body + integrateRigidBodiesPositions(); - // Integrate the position and orientation of each body - integrateRigidBodiesPositions(); + // Solve the position correction for constraints + solvePositionCorrection(); - // Solve the position correction for constraints - solvePositionCorrection(); + // Update the state (positions and velocities) of the bodies + updateBodiesState(); - // Update the state (positions and velocities) of the bodies - updateBodiesState(); + if (mIsSleepingEnabled) updateSleepingBodies(); - 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 resetBodiesForceAndTorque(); @@ -161,8 +153,6 @@ void DynamicsWorld::update() { void DynamicsWorld::integrateRigidBodiesPositions() { PROFILE("DynamicsWorld::integrateRigidBodiesPositions()"); - - decimal dt = static_cast(mTimer.getTimeStep()); // For each island of the world for (uint i=0; i < mNbIslands; i++) { @@ -190,10 +180,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() { const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); // Update the new constrained position and orientation of the body - mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt; + mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; mConstrainedOrientations[indexArray] = currentOrientation + 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::iterator it; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - - (*it)->setInterpolationFactor(factor); - } -} - // Initialize the bodies velocities arrays for the next simulation step. void DynamicsWorld::initVelocityArrays() { @@ -288,8 +261,7 @@ void DynamicsWorld::initVelocityArrays() { for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // Add the body into the map - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); indexBody++; } } @@ -306,8 +278,6 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // Initialize the bodies velocity arrays initVelocityArrays(); - decimal dt = static_cast(mTimer.getTimeStep()); - // For each island of the world for (uint i=0; i < mNbIslands; i++) { @@ -324,16 +294,16 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // Integrate the external force to get the new velocity of the body mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + - dt * bodies[b]->mMassInverse * bodies[b]->mExternalForce; + mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce; mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() + - dt * bodies[b]->getInertiaTensorInverseWorld() * + mTimeStep * bodies[b]->getInertiaTensorInverseWorld() * bodies[b]->mExternalTorque; // If the gravity has to be applied to this rigid body if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) { // Integrate the gravity force - mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->mMassInverse * + mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse * bodies[b]->getMass() * mGravity; } @@ -352,9 +322,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // => v2 = v1 * (1 - c * dt) decimal linDampingFactor = bodies[b]->getLinearDamping(); 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 angularDamping = clamp(decimal(1.0) - dt * angDampingFactor, + decimal angularDamping = clamp(decimal(1.0) - mTimeStep * angDampingFactor, decimal(0.0), decimal(1.0)); mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0), decimal(1.0)); @@ -374,9 +344,6 @@ void DynamicsWorld::solveContactsAndConstraints() { PROFILE("DynamicsWorld::solveContactsAndConstraints()"); - // Get the current time step - decimal dt = static_cast(mTimer.getTimeStep()); - // Set the velocities arrays mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, @@ -400,7 +367,7 @@ void DynamicsWorld::solveContactsAndConstraints() { if (isContactsToSolve) { // Initialize the solver - mContactSolver.initializeForIsland(dt, mIslands[islandIndex]); + mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); // Warm start the contact solver mContactSolver.warmStart(); @@ -410,7 +377,7 @@ void DynamicsWorld::solveContactsAndConstraints() { if (isConstraintsToSolve) { // Initialize the constraint solver - mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]); + mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); } // For each iteration of the velocity solver @@ -812,7 +779,6 @@ void DynamicsWorld::updateSleepingBodies() { PROFILE("DynamicsWorld::updateSleepingBodies()"); - const decimal dt = static_cast(mTimer.getTimeStep()); const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; @@ -840,7 +806,7 @@ void DynamicsWorld::updateSleepingBodies() { else { // If the body velocity is bellow the sleeping velocity threshold // Increase the sleep time - bodies[b]->mSleepTime += dt; + bodies[b]->mSleepTime += mTimeStep; if (bodies[b]->mSleepTime < minSleepTime) { minSleepTime = bodies[b]->mSleepTime; } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b03df610..bb0e9acb 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -32,7 +32,6 @@ #include "ContactSolver.h" #include "ConstraintSolver.h" #include "body/RigidBody.h" -#include "Timer.h" #include "Island.h" #include "configuration.h" @@ -51,9 +50,6 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Attributes -------------------- // - /// Timer of the physics engine - Timer mTimer; - /// Contact solver ContactSolver mContactSolver; @@ -78,6 +74,9 @@ class DynamicsWorld : public CollisionWorld { /// Gravity vector of the world Vector3 mGravity; + /// Current frame time step (in seconds) + decimal mTimeStep; + /// True if the gravity force is on bool mIsGravityEnabled; @@ -182,7 +181,7 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Methods -------------------- // /// Constructor - DynamicsWorld(const Vector3& mGravity, decimal timeStep); + DynamicsWorld(const Vector3& mGravity); /// Destructor virtual ~DynamicsWorld(); @@ -194,7 +193,7 @@ class DynamicsWorld : public CollisionWorld { void stop(); /// Update the physics simulation - void update(); + void update(decimal timeStep); /// Set the number of iterations for the velocity constraint solver void setNbIterationsVelocitySolver(uint nbIterations); @@ -312,16 +311,7 @@ inline void DynamicsWorld::resetBodiesForceAndTorque() { (*it)->mExternalForce.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 /** @@ -434,14 +424,6 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() 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 and false otherwise diff --git a/src/engine/Timer.cpp b/src/engine/Timer.cpp deleted file mode 100644 index 30698fd4..00000000 --- a/src/engine/Timer.cpp +++ /dev/null @@ -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 -} - - - - - - - - - diff --git a/src/engine/Timer.h b/src/engine/Timer.h deleted file mode 100644 index be7a7cac..00000000 --- a/src/engine/Timer.h +++ /dev/null @@ -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 -#include -#include -#include -#include "configuration.h" - -#if defined(WINDOWS_OS) // For Windows platform - #define NOMINMAX // This is used to avoid definition of max() and min() macros - #include -#else // For Mac OS or Linux platform - #include -#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