From 9b38fc16264039d75b892143c816d5a6a27beb58 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 8 Aug 2019 17:45:22 +0200 Subject: [PATCH] Add DynamicsSystem class --- CMakeLists.txt | 10 +- src/body/CollisionBody.cpp | 6 +- src/body/CollisionBody.h | 2 +- src/body/RigidBody.h | 2 +- src/collision/CollisionDetection.h | 12 +- src/collision/ContactManifold.h | 2 +- src/collision/ProxyShape.cpp | 2 +- src/components/RigidBodyComponents.h | 3 +- src/constraint/BallAndSocketJoint.cpp | 2 +- src/constraint/ContactPoint.h | 2 +- src/constraint/FixedJoint.cpp | 2 +- src/constraint/HingeJoint.cpp | 2 +- src/constraint/Joint.h | 2 +- src/constraint/SliderJoint.cpp | 2 +- src/constraint/SliderJoint.h | 2 +- src/engine/DynamicsWorld.cpp | 192 +++--------------- src/engine/DynamicsWorld.h | 67 ++---- src/systems/BroadPhaseSystem.cpp | 17 +- src/systems/BroadPhaseSystem.h | 8 +- .../ConstraintSolverSystem.cpp} | 10 +- .../ConstraintSolverSystem.h} | 14 +- .../ContactSolverSystem.cpp} | 30 +-- .../ContactSolverSystem.h} | 20 +- src/systems/DynamicsSystem.cpp | 190 +++++++++++++++++ src/systems/DynamicsSystem.h | 111 ++++++++++ 25 files changed, 416 insertions(+), 296 deletions(-) rename src/{engine/ConstraintSolver.cpp => systems/ConstraintSolverSystem.cpp} (91%) rename src/{engine/ConstraintSolver.h => systems/ConstraintSolverSystem.h} (95%) rename src/{engine/ContactSolver.cpp => systems/ContactSolverSystem.cpp} (98%) rename src/{engine/ContactSolver.h => systems/ContactSolverSystem.h} (96%) create mode 100644 src/systems/DynamicsSystem.cpp create mode 100644 src/systems/DynamicsSystem.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e5a12319..00011295 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,8 +129,9 @@ SET (REACTPHYSICS3D_HEADERS "src/engine/Entity.h" "src/engine/EntityManager.h" "src/engine/CollisionWorld.h" - "src/engine/ConstraintSolver.h" - "src/engine/ContactSolver.h" + "src/systems/ConstraintSolverSystem.h" + "src/systems/ContactSolverSystem.h" + "src/systems/DynamicsSystem.h" "src/engine/DynamicsWorld.h" "src/engine/EventListener.h" "src/engine/Island.h" @@ -219,8 +220,9 @@ SET (REACTPHYSICS3D_SOURCES "src/constraint/Joint.cpp" "src/constraint/SliderJoint.cpp" "src/engine/CollisionWorld.cpp" - "src/engine/ConstraintSolver.cpp" - "src/engine/ContactSolver.cpp" + "src/systems/ConstraintSolverSystem.cpp" + "src/systems/ContactSolverSystem.cpp" + "src/systems/DynamicsSystem.cpp" "src/engine/DynamicsWorld.cpp" "src/engine/Island.cpp" "src/engine/Material.cpp" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index b436ddb5..b47ecca5 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -214,14 +214,14 @@ const Transform& CollisionBody::getTransform() const { } // Update the broad-phase state for this body (because it has moved for instance) -void CollisionBody::updateBroadPhaseState() const { +void CollisionBody::updateBroadPhaseState(decimal timeStep) const { // For all the proxy collision shapes of the body const List& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity); for (uint i=0; i < proxyShapesEntities.size(); i++) { // Update the proxy - mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i]); + mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i], timeStep); } } @@ -390,7 +390,7 @@ void CollisionBody::setTransform(const Transform& transform) { mWorld.mTransformComponents.setTransform(mEntity, transform); // Update the broad-phase state of the body - updateBroadPhaseState(); + updateBroadPhaseState(0); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set transform=" + transform.to_string()); diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 95d4b913..7101181f 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -82,7 +82,7 @@ class CollisionBody { void removeAllCollisionShapes(); /// Update the broad-phase state for this body (because it has moved for instance) - void updateBroadPhaseState() const; + void updateBroadPhaseState(decimal timeStep) const; /// Ask the broad-phase to test again the collision shapes of the body for collision /// (as if the body has moved). diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index f676755a..6ab29c5f 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -218,7 +218,7 @@ class RigidBody : public CollisionBody { // -------------------- Friendship -------------------- // friend class DynamicsWorld; - friend class ContactSolver; + friend class ContactSolverSystem; friend class BallAndSocketJoint; friend class SliderJoint; friend class HingeJoint; diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index dd91b296..a9e9a2c6 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -292,10 +292,10 @@ class CollisionDetection { void removeProxyCollisionShape(ProxyShape* proxyShape); /// Update a proxy collision shape (that has moved for instance) - void updateProxyShape(Entity proxyShapeEntity); + void updateProxyShape(Entity proxyShapeEntity, decimal timeStep); /// Update all the enabled proxy-shapes - void updateProxyShapes(); + void updateProxyShapes(decimal timeStep); /// Add a pair of bodies that cannot collide with each other void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); @@ -411,15 +411,15 @@ inline MemoryManager& CollisionDetection::getMemoryManager() const { } // Update a proxy collision shape (that has moved for instance) -inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity) { +inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity, decimal timeStep) { // Update the proxy-shape component - mBroadPhaseSystem.updateProxyShape(proxyShapeEntity); + mBroadPhaseSystem.updateProxyShape(proxyShapeEntity, timeStep); } // Update all the enabled proxy-shapes -inline void CollisionDetection::updateProxyShapes() { - mBroadPhaseSystem.updateProxyShapes(); +inline void CollisionDetection::updateProxyShapes(decimal timeStep) { + mBroadPhaseSystem.updateProxyShapes(timeStep); } #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 4f7df13f..abc5fed8 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -124,7 +124,7 @@ class ContactManifold { friend class Island; friend class CollisionBody; friend class ContactManifoldSet; - friend class ContactSolver; + friend class ContactSolverSystem; friend class CollisionDetection; }; diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 052ee301..e46ec63a 100644 --- a/src/collision/ProxyShape.cpp +++ b/src/collision/ProxyShape.cpp @@ -122,7 +122,7 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) { mBody->mWorld.mRigidBodyComponents.setIsSleeping(mBody->getEntity(), false); } - mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity); + mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity, 0); int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity); diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index d7f4d174..640a234b 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -333,7 +333,8 @@ class RigidBodyComponents : public Components { // -------------------- Friendship -------------------- // friend class DynamicsWorld; - friend class ContactSolver; + friend class ContactSolverSystem; + friend class DynamicsSystem; friend class BallAndSocketJoint; friend class FixedJoint; friend class HingeJoint; diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 171f39b9..a5b100a2 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -25,7 +25,7 @@ // Libraries #include "BallAndSocketJoint.h" -#include "engine/ConstraintSolver.h" +#include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index c681396d..a6adb8bb 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -136,7 +136,7 @@ class ContactPoint { // Friendship friend class ContactManifold; friend class ContactManifoldSet; - friend class ContactSolver; + friend class ContactSolverSystem; friend class CollisionDetection; }; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index f954da20..d317d237 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -25,7 +25,7 @@ // Libraries #include "FixedJoint.h" -#include "engine/ConstraintSolver.h" +#include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 469fc124..5ae7755a 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -25,7 +25,7 @@ // Libraries #include "HingeJoint.h" -#include "engine/ConstraintSolver.h" +#include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 38e26b5d..e21f0263 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -213,7 +213,7 @@ class Joint { friend class DynamicsWorld; friend class Island; - friend class ConstraintSolver; + friend class ConstraintSolverSystem; }; // Return the reference to the body 1 diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 02026d30..86864d45 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -25,7 +25,7 @@ // Libraries #include "SliderJoint.h" -#include "engine/ConstraintSolver.h" +#include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 5720915e..b9a83de0 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -34,7 +34,7 @@ namespace reactphysics3d { // Declarations -class ConstraintSolver; +class ConstraintSolverSystem; // Structure SliderJointInfo /** diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 9be94ee6..adcc1a60 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,13 +50,14 @@ using namespace std; DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) : CollisionWorld(worldSettings, logger, profiler), mIslands(mMemoryManager.getSingleFrameAllocator()), - mContactSolver(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, + mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mProxyShapesComponents, mConfig), - mConstraintSolver(mIslands, mRigidBodyComponents), + mConstraintSolverSystem(mIslands, mRigidBodyComponents), + mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), - mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), + mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { @@ -64,8 +65,9 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS #ifdef IS_PROFILING_ACTIVE // Set the profiler - mConstraintSolver.setProfiler(mProfiler); - mContactSolver.setProfiler(mProfiler); + mConstraintSolverSystem.setProfiler(mProfiler); + mContactSolverSystem.setProfiler(mProfiler); + mDynamicsSystem.setProfiler(mProfiler); #endif @@ -113,8 +115,6 @@ void DynamicsWorld::update(decimal timeStep) { RP3D_PROFILE("DynamicsWorld::update()", mProfiler); - mTimeStep = timeStep; - // Notify the event listener about the beginning of an internal tick if (mEventListener != nullptr) mEventListener->beginInternalTick(); @@ -131,27 +131,30 @@ void DynamicsWorld::update(decimal timeStep) { mCollisionDetection.reportContacts(); // Integrate the velocities - integrateRigidBodiesVelocities(); + mDynamicsSystem.integrateRigidBodiesVelocities(timeStep); // Solve the contacts and constraints - solveContactsAndConstraints(); + solveContactsAndConstraints(timeStep); // Integrate the position and orientation of each body - integrateRigidBodiesPositions(); + mDynamicsSystem.integrateRigidBodiesPositions(timeStep, mContactSolverSystem.isSplitImpulseActive()); // Solve the position correction for constraints solvePositionCorrection(); // Update the state (positions and velocities) of the bodies - updateBodiesState(); + mDynamicsSystem.updateBodiesState(); - if (mIsSleepingEnabled) updateSleepingBodies(); + // Update the proxy-shapes components + mCollisionDetection.updateProxyShapes(timeStep); + + if (mIsSleepingEnabled) updateSleepingBodies(timeStep); // Notify the event listener about the end of an internal tick if (mEventListener != nullptr) mEventListener->endInternalTick(); // Reset the external force and torque applied to the bodies - resetBodiesForceAndTorque(); + mDynamicsSystem.resetBodiesForceAndTorque(); // Reset the islands mIslands.clear(); @@ -160,161 +163,16 @@ void DynamicsWorld::update(decimal timeStep) { mMemoryManager.resetFrameAllocator(); } -// Integrate position and orientation of the rigid bodies. -/// The positions and orientations of the bodies are integrated using -/// the sympletic Euler time stepping scheme. -void DynamicsWorld::integrateRigidBodiesPositions() { - - RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler); - - const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0); - - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - // Get the constrained velocity - Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i]; - Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i]; - - // Add the split impulse velocity from Contact Solver (only used - // to update the position) - newLinVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitLinearVelocities[i]; - newAngVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitAngularVelocities[i]; - - // Get current position and orientation of the body - const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i]; - const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation(); - - // Update the new constrained position and orientation of the body - mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep; - mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * mTimeStep; - } -} - -// Update the postion/orientation of the bodies -void DynamicsWorld::updateBodiesState() { - - RP3D_PROFILE("DynamicsWorld::updateBodiesState()", mProfiler); - - // TODO : Make sure we compute this in a system - - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - // Update the linear and angular velocity of the body - mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]); - mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]); - - // Update the position of the center of mass of the body - mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i]; - - // Update the orientation of the body - const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i]; - mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit()); - } - - // Update the transform of the body (using the new center of mass and new orientation) - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]); - const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i]; - const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i]; - transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); - } - - // Update the world inverse inertia tensor of the body - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix(); - const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i]; - mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose(); - } - - // Update the proxy-shapes components - mCollisionDetection.updateProxyShapes(); -} - -// Reset the split velocities of the bodies -void DynamicsWorld::resetSplitVelocities() { - - for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - mRigidBodyComponents.mSplitLinearVelocities[i].setToZero(); - mRigidBodyComponents.mSplitAngularVelocities[i].setToZero(); - } -} - -// Integrate the velocities of rigid bodies. -/// This method only set the temporary velocities but does not update -/// the actual velocitiy of the bodies. The velocities updated in this method -/// might violate the constraints and will be corrected in the constraint and -/// contact solver. -void DynamicsWorld::integrateRigidBodiesVelocities() { - - RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler); - - // Reset the split velocities of the bodies - resetSplitVelocities(); - - // Integration component velocities using force/torque - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0)); - assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0)); - - const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i]; - const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i]; - - // Integrate the external force to get the new velocity of the body - mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep * - mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; - mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep * - mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]; - } - - // Apply gravity force - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - // If the gravity has to be applied to this rigid body - if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) { - - // Integrate the gravity force - mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + mTimeStep * - mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity; - } - } - - // Apply the velocity damping - // Damping force : F_c = -c' * v (c=damping factor) - // Equation : m * dv/dt = -c' * v - // => dv/dt = -c * v (with c=c'/m) - // => dv/dt + c * v = 0 - // Solution : v(t) = v0 * e^(-c * t) - // => v(t + dt) = v0 * e^(-c(t + dt)) - // = v0 * e^(-ct) * e^(-c * dt) - // = v(t) * e^(-c * dt) - // => v2 = v1 * e^(-c * dt) - // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... - // => e^(-x) ~ 1 - x - // => v2 = v1 * (1 - c * dt) - for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { - - const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i]; - const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i]; - const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); - const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); - mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping; - mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping; - } -} // Solve the contacts and constraints -void DynamicsWorld::solveContactsAndConstraints() { +void DynamicsWorld::solveContactsAndConstraints(decimal timeStep) { RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler); // ---------- Solve velocity constraints for joints and contacts ---------- // // Initialize the contact solver - mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep); + mContactSolverSystem.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, timeStep); // For each island of the world for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { @@ -323,7 +181,7 @@ void DynamicsWorld::solveContactsAndConstraints() { if (mIslands.joints[islandIndex].size() > 0) { // Initialize the constraint solver - mConstraintSolver.initializeForIsland(mTimeStep, islandIndex); + mConstraintSolverSystem.initializeForIsland(timeStep, islandIndex); } } @@ -335,14 +193,14 @@ void DynamicsWorld::solveContactsAndConstraints() { // Solve the constraints if (mIslands.joints[islandIndex].size() > 0) { - mConstraintSolver.solveVelocityConstraints(islandIndex); + mConstraintSolverSystem.solveVelocityConstraints(islandIndex); } } - mContactSolver.solve(); + mContactSolverSystem.solve(); } - mContactSolver.storeImpulses(); + mContactSolverSystem.storeImpulses(); } // Solve the position error correction of the constraints @@ -364,7 +222,7 @@ void DynamicsWorld::solvePositionCorrection() { for (uint i=0; i 0); assert(startIndex < mProxyShapesComponents.getNbComponents()); @@ -158,13 +158,6 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbI nbItems = endIndex - startIndex; assert(nbItems >= 0); - // Get the time step if we are in a dynamics world - DynamicsWorld* dynamicsWorld = dynamic_cast(mCollisionDetection.getWorld()); - decimal timeStep = decimal(0.0); - if (dynamicsWorld != nullptr) { - timeStep = dynamicsWorld->getTimeStep(); - } - // For each proxy-shape component to update for (uint32 i = startIndex; i < startIndex + nbItems; i++) { diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index 62484033..a5bef448 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/src/systems/BroadPhaseSystem.h @@ -97,7 +97,7 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { }; -// Class BroadPhaseAlgorithm +// Class BroadPhaseSystem /** * This class represents the broad-phase collision detection. The * goal of the broad-phase collision detection is to compute the pairs of proxy shapes @@ -143,7 +143,7 @@ class BroadPhaseSystem { void updateProxyShapeInternal(int broadPhaseId, const AABB& aabb, const Vector3& displacement); /// Update the broad-phase state of some proxy-shapes components - void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems); + void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep); public : @@ -169,10 +169,10 @@ class BroadPhaseSystem { void removeProxyCollisionShape(ProxyShape* proxyShape); /// Update the broad-phase state of a single proxy-shape - void updateProxyShape(Entity proxyShapeEntity); + void updateProxyShape(Entity proxyShapeEntity, decimal timeStep); /// Update the broad-phase state of all the enabled proxy-shapes - void updateProxyShapes(); + void updateProxyShapes(decimal timeStep); /// Add a collision shape in the array of shapes that have moved in the last simulation step /// and that need to be tested again for broad-phase overlapping. diff --git a/src/engine/ConstraintSolver.cpp b/src/systems/ConstraintSolverSystem.cpp similarity index 91% rename from src/engine/ConstraintSolver.cpp rename to src/systems/ConstraintSolverSystem.cpp index 14979cff..0afe523a 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/systems/ConstraintSolverSystem.cpp @@ -24,14 +24,14 @@ ********************************************************************************/ // Libraries -#include "ConstraintSolver.h" +#include "systems/ConstraintSolverSystem.h" #include "utils/Profiler.h" #include "engine/Island.h" using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents) +ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents) : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents) { #ifdef IS_PROFILING_ACTIVE @@ -43,7 +43,7 @@ ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidB } // Initialize the constraint solver for a given island -void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) { +void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) { RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler); @@ -71,7 +71,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) { } // Solve the velocity constraints -void ConstraintSolver::solveVelocityConstraints(uint islandIndex) { +void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) { RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler); @@ -86,7 +86,7 @@ void ConstraintSolver::solveVelocityConstraints(uint islandIndex) { } // Solve the position constraints -void ConstraintSolver::solvePositionConstraints(uint islandIndex) { +void ConstraintSolverSystem::solvePositionConstraints(uint islandIndex) { RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler); diff --git a/src/engine/ConstraintSolver.h b/src/systems/ConstraintSolverSystem.h similarity index 95% rename from src/engine/ConstraintSolver.h rename to src/systems/ConstraintSolverSystem.h index 069e8b89..6898f544 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/systems/ConstraintSolverSystem.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_H -#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H +#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H +#define REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H // Libraries #include "configuration.h" @@ -66,7 +66,7 @@ struct ConstraintSolverData { }; -// Class ConstraintSolver +// Class ConstraintSolverSystem /** * This class represents the constraint solver that is used to solve constraints between * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique @@ -135,7 +135,7 @@ struct ConstraintSolverData { * friction but also another twist friction constraint to prevent spin of the body around the * contact manifold center. */ -class ConstraintSolver { +class ConstraintSolverSystem { private : @@ -164,10 +164,10 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents); + ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents); /// Destructor - ~ConstraintSolver() = default; + ~ConstraintSolverSystem() = default; /// Initialize the constraint solver for a given island void initializeForIsland(decimal dt, uint islandIndex); @@ -196,7 +196,7 @@ class ConstraintSolver { #ifdef IS_PROFILING_ACTIVE // Set the profiler -inline void ConstraintSolver::setProfiler(Profiler* profiler) { +inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/src/engine/ContactSolver.cpp b/src/systems/ContactSolverSystem.cpp similarity index 98% rename from src/engine/ContactSolver.cpp rename to src/systems/ContactSolverSystem.cpp index fe43473f..03e1a95c 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -24,8 +24,8 @@ ********************************************************************************/ // Libraries -#include "ContactSolver.h" -#include "DynamicsWorld.h" +#include "systems/ContactSolverSystem.h" +#include "engine/DynamicsWorld.h" #include "body/RigidBody.h" #include "constraint/ContactPoint.h" #include "utils/Profiler.h" @@ -38,12 +38,12 @@ using namespace reactphysics3d; using namespace std; // Constants initialization -const decimal ContactSolver::BETA = decimal(0.2); -const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP = decimal(0.01); +const decimal ContactSolverSystem::BETA = decimal(0.2); +const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2); +const decimal ContactSolverSystem::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, +ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings) :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr), @@ -59,7 +59,7 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, Col } // Initialize the contact constraints -void ContactSolver::init(List* contactManifolds, List* contactPoints, decimal timeStep) { +void ContactSolverSystem::init(List* contactManifolds, List* contactPoints, decimal timeStep) { mAllContactManifolds = contactManifolds; mAllContactPoints = contactPoints; @@ -102,7 +102,7 @@ void ContactSolver::init(List* contactManifolds, ListgetMaterial().getBounciness(); decimal restitution2 = body2->getMaterial().getBounciness(); @@ -774,7 +774,7 @@ decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, } // Compute the mixed friction coefficient from the friction coefficient of each body -decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, +decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody *body1, RigidBody *body2) const { // Use the geometric mean to compute the mixed friction coefficient return std::sqrt(body1->getMaterial().getFrictionCoefficient() * @@ -782,14 +782,14 @@ decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1, } // Compute th mixed rolling resistance factor between two bodies -inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1, +inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const { return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance()); } // Store the computed impulses to use them to // warm start the solver at the next iteration -void ContactSolver::storeImpulses() { +void ContactSolverSystem::storeImpulses() { RP3D_PROFILE("ContactSolver::storeImpulses()", mProfiler); @@ -816,7 +816,7 @@ void ContactSolver::storeImpulses() { // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, +void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); diff --git a/src/engine/ContactSolver.h b/src/systems/ContactSolverSystem.h similarity index 96% rename from src/engine/ContactSolver.h rename to src/systems/ContactSolverSystem.h index 76241379..15822803 100644 --- a/src/engine/ContactSolver.h +++ b/src/systems/ContactSolverSystem.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef REACTPHYSICS3D_CONTACT_SOLVER_H -#define REACTPHYSICS3D_CONTACT_SOLVER_H +#ifndef REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H +#define REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H // Libraries #include "configuration.h" @@ -48,9 +48,9 @@ class DynamicsComponents; class RigidBodyComponents; class ProxyShapeComponents; -// Class Contact Solver +// Class ContactSolverSystem /** - * This class represents the contact solver that is used to solve rigid bodies contacts. + * This class represents the contact solver system that is used to 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). * @@ -117,7 +117,7 @@ class ProxyShapeComponents; * friction but also another twist friction constraint to prevent spin of the body around the * contact manifold center. */ -class ContactSolver { +class ContactSolverSystem { private: @@ -360,12 +360,12 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, + ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings); /// Destructor - ~ContactSolver() = default; + ~ContactSolverSystem() = default; /// Initialize the contact constraints void init(List* contactManifolds, List* contactPoints, decimal timeStep); @@ -395,19 +395,19 @@ class ContactSolver { }; // Return true if the split impulses position correction technique is used for contacts -inline bool ContactSolver::isSplitImpulseActive() const { +inline bool ContactSolverSystem::isSplitImpulseActive() const { return mIsSplitImpulseActive; } // Activate or Deactivate the split impulses for contacts -inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { +inline void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) { mIsSplitImpulseActive = isActive; } #ifdef IS_PROFILING_ACTIVE // Set the profiler -inline void ContactSolver::setProfiler(Profiler* profiler) { +inline void ContactSolverSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; } diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp new file mode 100644 index 00000000..042f1c86 --- /dev/null +++ b/src/systems/DynamicsSystem.cpp @@ -0,0 +1,190 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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 "systems/DynamicsSystem.h" + +using namespace reactphysics3d; + +// Constructor +DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity) + :mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled), + mGravity(gravity) { + +} + +// Integrate position and orientation of the rigid bodies. +/// The positions and orientations of the bodies are integrated using +/// the sympletic Euler time stepping scheme. +void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive) { + + RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesPositions()", mProfiler); + + const decimal isSplitImpulseFactor = isSplitImpulseActive ? decimal(1.0) : decimal(0.0); + + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + // Get the constrained velocity + Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i]; + Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i]; + + // Add the split impulse velocity from Contact Solver (only used + // to update the position) + newLinVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitLinearVelocities[i]; + newAngVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitAngularVelocities[i]; + + // Get current position and orientation of the body + const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i]; + const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation(); + + // Update the new constrained position and orientation of the body + mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * timeStep; + mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) * + currentOrientation * decimal(0.5) * timeStep; + } +} + +// Update the postion/orientation of the bodies +void DynamicsSystem::updateBodiesState() { + + RP3D_PROFILE("DynamicsSystem::updateBodiesState()", mProfiler); + + // TODO : Make sure we compute this in a system + + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + // Update the linear and angular velocity of the body + mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]); + mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]); + + // Update the position of the center of mass of the body + mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i]; + + // Update the orientation of the body + const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i]; + mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit()); + } + + // Update the transform of the body (using the new center of mass and new orientation) + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]); + const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i]; + const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i]; + transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); + } + + // Update the world inverse inertia tensor of the body + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix(); + const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i]; + mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose(); + } +} + +// Integrate the velocities of rigid bodies. +/// This method only set the temporary velocities but does not update +/// the actual velocitiy of the bodies. The velocities updated in this method +/// might violate the constraints and will be corrected in the constraint and +/// contact solver. +void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { + + RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesVelocities()", mProfiler); + + // Reset the split velocities of the bodies + resetSplitVelocities(); + + // Integration component velocities using force/torque + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0)); + assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0)); + + const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i]; + const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i]; + + // Integrate the external force to get the new velocity of the body + mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * + mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * + mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]; + } + + // Apply gravity force + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + // If the gravity has to be applied to this rigid body + if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) { + + // Integrate the gravity force + mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep * + mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * mGravity; + } + } + + // Apply the velocity damping + // Damping force : F_c = -c' * v (c=damping factor) + // Equation : m * dv/dt = -c' * v + // => dv/dt = -c * v (with c=c'/m) + // => dv/dt + c * v = 0 + // Solution : v(t) = v0 * e^(-c * t) + // => v(t + dt) = v0 * e^(-c(t + dt)) + // = v0 * e^(-ct) * e^(-c * dt) + // = v(t) * e^(-c * dt) + // => v2 = v1 * e^(-c * dt) + // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... + // => e^(-x) ~ 1 - x + // => v2 = v1 * (1 - c * dt) + for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + + const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i]; + const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i]; + const decimal linearDamping = std::pow(decimal(1.0) - linDampingFactor, timeStep); + const decimal angularDamping = std::pow(decimal(1.0) - angDampingFactor, timeStep); + mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping; + mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping; + } +} + +// Reset the external force and torque applied to the bodies +void DynamicsSystem::resetBodiesForceAndTorque() { + + // For each body of the world + for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) { + mRigidBodyComponents.mExternalForces[i].setToZero(); + mRigidBodyComponents.mExternalTorques[i].setToZero(); + } +} + +// Reset the split velocities of the bodies +void DynamicsSystem::resetSplitVelocities() { + + for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) { + mRigidBodyComponents.mSplitLinearVelocities[i].setToZero(); + mRigidBodyComponents.mSplitAngularVelocities[i].setToZero(); + } +} + diff --git a/src/systems/DynamicsSystem.h b/src/systems/DynamicsSystem.h new file mode 100644 index 00000000..44cef67d --- /dev/null +++ b/src/systems/DynamicsSystem.h @@ -0,0 +1,111 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 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_DYNAMICS_SYSTEM_H +#define REACTPHYSICS3D_DYNAMICS_SYSTEM_H + +// Libraries +#include "utils/Profiler.h" +#include "components/RigidBodyComponents.h" +#include "components/TransformComponents.h" + +namespace reactphysics3d { + +// Class DynamicsSystem +/** + * This class is responsible to compute and update the dynamics of the bodies that are simulated + * using physics. + */ +class DynamicsSystem { + + private : + + // -------------------- Attributes -------------------- // + + /// Reference to the rigid body components + RigidBodyComponents& mRigidBodyComponents; + + /// Reference to the transform components + TransformComponents& mTransformComponents; + + /// Reference to the variable to know if gravity is enabled in the world + bool& mIsGravityEnabled; + + /// Reference to the world gravity vector + Vector3& mGravity; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; +#endif + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, + bool& isGravityEnabled, Vector3& gravity); + + /// Destructor + ~DynamicsSystem() = default; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + + /// Integrate the positions and orientations of rigid bodies. + void integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive); + + /// Integrate the velocities of rigid bodies. + void integrateRigidBodiesVelocities(decimal timeStep); + + /// Update the postion/orientation of the bodies + void updateBodiesState(); + + /// Reset the external force and torque applied to the bodies + void resetBodiesForceAndTorque(); + + /// Reset the split velocities of the bodies + void resetSplitVelocities(); + +}; + +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void DynamicsSystem::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + +} + +#endif