From fe81c3fd6e017c1afb3943e837b3bf24f082762f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 21 Mar 2019 07:29:59 +0100 Subject: [PATCH] =?UTF-8?q?Add=C2=A0DynamicsComponents=20class?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + src/body/RigidBody.cpp | 62 ++++++++-- src/body/RigidBody.h | 33 +----- src/collision/CollisionDetection.cpp | 7 +- src/collision/CollisionDetection.h | 3 +- src/components/Components.h | 10 +- src/components/DynamicsComponents.cpp | 159 ++++++++++++++++++++++++++ src/components/DynamicsComponents.h | 153 +++++++++++++++++++++++++ src/engine/CollisionWorld.cpp | 3 +- src/engine/CollisionWorld.h | 4 + src/engine/DynamicsWorld.cpp | 7 +- src/engine/DynamicsWorld.h | 12 ++ src/systems/BroadPhaseSystem.cpp | 26 ++++- src/systems/BroadPhaseSystem.h | 6 +- 14 files changed, 430 insertions(+), 57 deletions(-) create mode 100644 src/components/DynamicsComponents.cpp create mode 100644 src/components/DynamicsComponents.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b52047ab..63cc8a17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,6 +143,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/BodyComponents.h" "src/components/TransformComponents.h" "src/components/ProxyShapeComponents.h" + "src/components/DynamicsComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -235,6 +236,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/BodyComponents.cpp" "src/components/TransformComponents.cpp" "src/components/ProxyShapeComponents.cpp" + "src/components/DynamicsComponents.cpp" "src/collision/CollisionCallback.cpp" "src/mathematics/mathematics_functions.cpp" "src/mathematics/Matrix2x2.cpp" diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index ad47335d..a456bdbe 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -83,8 +83,8 @@ void RigidBody::setType(BodyType type) { if (mType == BodyType::STATIC) { // Reset the velocity to zero - mLinearVelocity.setToZero(); - mAngularVelocity.setToZero(); + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero()); + mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero()); } // If it is a static or a kinematic body @@ -189,7 +189,10 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal; // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mID) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string()); @@ -422,15 +425,15 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { if (mType == BodyType::STATIC) return; // Update the linear velocity of the current body state - mLinearVelocity = linearVelocity; + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); // If the linear velocity is not zero, awake the body - if (mLinearVelocity.lengthSquare() > decimal(0.0)) { + if (linearVelocity.lengthSquare() > decimal(0.0)) { setIsSleeping(false); } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set linearVelocity=" + mLinearVelocity.to_string()); + "Body " + std::to_string(mID) + ": Set linearVelocity=" + linearVelocity.to_string()); } // Set the angular velocity. @@ -439,19 +442,21 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { */ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { + // TODO : Make sure this method is not called from the internal physics engine + // If it is a static body, we do nothing if (mType == BodyType::STATIC) return; // Set the angular velocity - mAngularVelocity = angularVelocity; + mWorld.mDynamicsComponents.setAngularVelocity(mEntity, angularVelocity); // If the velocity is not zero, awake the body - if (mAngularVelocity.lengthSquare() > decimal(0.0)) { + if (angularVelocity.lengthSquare() > decimal(0.0)) { setIsSleeping(false); } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set angularVelocity=" + mAngularVelocity.to_string()); + "Body " + std::to_string(mID) + ": Set angularVelocity=" + angularVelocity.to_string()); } // Set the current position and orientation @@ -470,7 +475,10 @@ void RigidBody::setTransform(const Transform& transform) { mCenterOfMassWorld = transform * mCenterOfMassLocal; // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); + const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -573,7 +581,39 @@ void RigidBody::recomputeMassInformation() { updateInertiaTensorInverseWorld(); // Update the linear velocity of the center of mass - mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); + Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); +} + +// Return the linear velocity +/** + * @return The linear velocity vector of the body + */ +Vector3 RigidBody::getLinearVelocity() const { + return mWorld.mDynamicsComponents.getLinearVelocity(mEntity); +} + +// Return the angular velocity of the body +/** + * @return The angular velocity vector of the body + */ +Vector3 RigidBody::getAngularVelocity() const { + return mWorld.mDynamicsComponents.getAngularVelocity(mEntity); +} + +// Set the variable to know whether or not the body is sleeping +void RigidBody::setIsSleeping(bool isSleeping) { + + if (isSleeping) { + mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero()); + mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero()); + mExternalForce.setToZero(); + mExternalTorque.setToZero(); + } + + CollisionBody::setIsSleeping(isSleeping); } #ifdef IS_PROFILING_ACTIVE diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index c23d0356..5946e990 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -70,10 +70,10 @@ class RigidBody : public CollisionBody { Vector3 mCenterOfMassWorld; /// Linear velocity of the body - Vector3 mLinearVelocity; + //Vector3 mLinearVelocity; /// Angular velocity of the body - Vector3 mAngularVelocity; + //Vector3 mAngularVelocity; /// Current external force on the body Vector3 mExternalForce; @@ -260,22 +260,6 @@ inline decimal RigidBody::getMass() const { return mInitMass; } -// Return the linear velocity -/** - * @return The linear velocity vector of the body - */ -inline Vector3 RigidBody::getLinearVelocity() const { - return mLinearVelocity; -} - -// Return the angular velocity of the body -/** - * @return The angular velocity vector of the body - */ -inline Vector3 RigidBody::getAngularVelocity() const { - return mAngularVelocity; -} - // Get the inverse local inertia tensor of the body (in body coordinates) inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { return mInertiaTensorLocalInverse; @@ -345,19 +329,6 @@ inline JointListElement* RigidBody::getJointsList() { return mJointsList; } -// Set the variable to know whether or not the body is sleeping -inline void RigidBody::setIsSleeping(bool isSleeping) { - - if (isSleeping) { - mLinearVelocity.setToZero(); - mAngularVelocity.setToZero(); - mExternalForce.setToZero(); - mExternalTorque.setToZero(); - } - - CollisionBody::setIsSleeping(isSleeping); -} - // Apply an external force to the body at its center of mass. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied forces and that this sum will be diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index af17bd42..92c5e36c 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -48,11 +48,12 @@ using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents &transformComponents, - MemoryManager& memoryManager) +CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents, + DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager) : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), - mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents), + mOverlappingPairs(mMemoryManager.getPoolAllocator()), + mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false), mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()) { diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 9357c0fe..b9c7e249 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -154,7 +154,8 @@ class CollisionDetection { /// Constructor CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents, MemoryManager& memoryManager); + TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents, + MemoryManager& memoryManager); /// Destructor ~CollisionDetection() = default; diff --git a/src/components/Components.h b/src/components/Components.h index cd5dcc56..d0bdf129 100644 --- a/src/components/Components.h +++ b/src/components/Components.h @@ -107,9 +107,12 @@ class Components { /// Remove a component void removeComponent(Entity entity); - // Notify if a given entity is disabled (sleeping or inactive) or not + /// Notify if a given entity is disabled (sleeping or inactive) or not void setIsEntityDisabled(Entity entity, bool isDisabled); + /// Return true if there is a component for a given entity + bool hasComponent(Entity entity) const; + /// Return the number of components uint32 getNbComponents() const; @@ -117,6 +120,11 @@ class Components { uint32 getNbEnabledComponents() const; }; +// Return true if there is a component for a given entity +inline bool Components::hasComponent(Entity entity) const { + return mMapEntityToComponentIndex.containsKey(entity); +} + // Return the number of components inline uint32 Components::getNbComponents() const { return mNbComponents; diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp new file mode 100644 index 00000000..effe55d6 --- /dev/null +++ b/src/components/DynamicsComponents.cpp @@ -0,0 +1,159 @@ +/******************************************************************************** +* 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 "DynamicsComponents.h" +#include "engine/EntityManager.h" +#include +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + + +// Constructor +DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + +// Allocate memory for a given number of components +void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { + + assert(nbComponentsToAllocate > mNbAllocatedComponents); + + // Size for the data of a single component (in bytes) + const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize; + + // Allocate memory + void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes); + assert(newBuffer != nullptr); + + // New pointers to components data + Entity* newEntities = static_cast(newBuffer); + Vector3* newLinearVelocities = reinterpret_cast(newEntities + nbComponentsToAllocate); + Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); + + // If there was already components before + if (mNbComponents > 0) { + + // Copy component data from the previous buffer to the new one + memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Transform)); + memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Entity)); + + // Deallocate previous memory + mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); + } + + mBuffer = newBuffer; + mBodies = newEntities; + mLinearVelocities = newLinearVelocities; + mAngularVelocities = newAngularVelocities; + mNbAllocatedComponents = nbComponentsToAllocate; +} + +// Add a component +void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component) { + + // Prepare to add new component (allocate memory if necessary and compute insertion index) + uint32 index = prepareAddComponent(isSleeping); + + // Insert the new component data + new (mBodies + index) Entity(bodyEntity); + new (mLinearVelocities + index) Vector3(component.linearVelocity); + new (mAngularVelocities + index) Vector3(component.angularVelocity); + + // Map the entity with the new component lookup index + mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); + + mNbComponents++; + + assert(mDisabledStartIndex <= mNbComponents); + assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); +} + +// Move a component from a source to a destination index in the components array +// The destination location must contain a constructed object +void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { + + const Entity entity = mBodies[srcIndex]; + + // Copy the data of the source component to the destination location + new (mBodies + destIndex) Entity(mBodies[srcIndex]); + new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]); + new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]); + + // Destroy the source component + destroyComponent(srcIndex); + + assert(!mMapEntityToComponentIndex.containsKey(entity)); + + // Update the entity to component index mapping + mMapEntityToComponentIndex.add(Pair(entity, destIndex)); + + assert(mMapEntityToComponentIndex[mBodies[destIndex]] == destIndex); +} + +// Swap two components in the array +void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity entity1(mBodies[index1]); + Vector3 linearVelocity1(mLinearVelocities[index1]); + Vector3 angularVelocity1(mAngularVelocities[index1]); + + // Destroy component 1 + destroyComponent(index1); + + moveComponentToIndex(index2, index1); + + // Reconstruct component 1 at component 2 location + new (mBodies + index2) Entity(entity1); + new (mLinearVelocities + index2) Vector3(linearVelocity1); + new (mAngularVelocities + index2) Vector3(angularVelocity1); + + // Update the entity to component index mapping + mMapEntityToComponentIndex.add(Pair(entity1, index2)); + + assert(mMapEntityToComponentIndex[mBodies[index1]] == index1); + assert(mMapEntityToComponentIndex[mBodies[index2]] == index2); + assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); +} + +// Destroy a component at a given index +void DynamicsComponents::destroyComponent(uint32 index) { + + Components::destroyComponent(index); + + assert(mMapEntityToComponentIndex[mBodies[index]] == index); + + mMapEntityToComponentIndex.remove(mBodies[index]); + + mBodies[index].~Entity(); + mLinearVelocities[index].~Vector3(); + mAngularVelocities[index].~Vector3(); +} diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h new file mode 100644 index 00000000..7c81e61b --- /dev/null +++ b/src/components/DynamicsComponents.h @@ -0,0 +1,153 @@ +/******************************************************************************** +* 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_COMPONENTS_H +#define REACTPHYSICS3D_DYNAMICS_COMPONENTS_H + +// Libraries +#include "mathematics/Transform.h" +#include "engine/Entity.h" +#include "components/Components.h" +#include "containers/Map.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class declarations +class MemoryAllocator; +class EntityManager; + +// Class DynamicsComponents +/** + * This class represent the component of the ECS that contains the variables concerning dynamics + * like velocities. + */ +class DynamicsComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of body entities of each component + Entity* mBodies; + + /// Array with the linear velocity of each component + Vector3* mLinearVelocities; + + /// Array with the angular velocity of each component + Vector3* mAngularVelocities; + + // -------------------- Methods -------------------- // + + /// Allocate memory for a given number of components + virtual void allocate(uint32 nbComponentsToAllocate) override; + + /// Destroy a component at a given index + virtual void destroyComponent(uint32 index) override; + + /// Move a component from a source to a destination index in the components array + virtual void moveComponentToIndex(uint32 srcIndex, uint32 destIndex) override; + + /// Swap two components in the array + virtual void swapComponents(uint32 index1, uint32 index2) override; + + public: + + /// Structure for the data of a transform component + struct DynamicsComponent { + + const Vector3& linearVelocity; + const Vector3& angularVelocity; + + /// Constructor + DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity) + : linearVelocity(linearVelocity), angularVelocity(angularVelocity) { + + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + DynamicsComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~DynamicsComponents() override = default; + + /// Add a component + void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component); + + /// Return the linear velocity of an entity + Vector3& getLinearVelocity(Entity bodyEntity) const; + + /// Return the angular velocity of an entity + Vector3& getAngularVelocity(Entity bodyEntity) const; + + /// Set the linear velocity of an entity + void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity); + + /// Set the angular velocity of an entity + void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity); + + // -------------------- Friendship -------------------- // + + friend class BroadPhaseSystem; +}; + +// Return the linear velocity of an entity +inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the angular velocity of an entity +inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the linear velocity of an entity +inline void DynamicsComponents::setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = linearVelocity; +} + +// Set the angular velocity of an entity +inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity; +} + +} + +#endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 36ef4b33..5ae61e12 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -39,7 +39,8 @@ uint CollisionWorld::mNbWorlds = 0; CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) : mConfig(worldSettings), mEntityManager(mMemoryManager.getPoolAllocator()), mBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()), - mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mMemoryManager), + mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mDynamicsComponents(mMemoryManager.getBaseAllocator()), + mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mDynamicsComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mCurrentBodyId(0), mFreeBodiesIds(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 87ebebce..3bf33372 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -36,6 +36,7 @@ #include "components/BodyComponents.h" #include "components/TransformComponents.h" #include "components/ProxyShapeComponents.h" +#include "components/DynamicsComponents.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -82,6 +83,9 @@ class CollisionWorld { /// Proxy-Shapes Components ProxyShapeComponents mProxyShapesComponents; + /// Dynamics components of the bodies (linear, angular velocities) + DynamicsComponents mDynamicsComponents; + /// Reference to the collision detection CollisionDetection mCollisionDetection; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index b14d250a..15524353 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,7 +50,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS : CollisionWorld(worldSettings, logger, profiler), mContactSolver(mMemoryManager, mConfig), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), - mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), + mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), @@ -214,8 +214,8 @@ void DynamicsWorld::updateBodiesState() { uint index = bodies[b]->mArrayIndex; // Update the linear and angular velocity of the body - bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index]; - bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index]; + mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]); + mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]); // Update the position of the center of mass of the body bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index]; @@ -429,6 +429,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform)); + mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero())); // Create the rigid body RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 49ded681..f17aaefd 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -32,6 +32,7 @@ #include "configuration.h" #include "utils/Logger.h" #include "engine/ContactSolver.h" +#include "components/DynamicsComponents.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -216,6 +217,9 @@ class DynamicsWorld : public CollisionWorld { /// Set the gravity vector of the world void setGravity(Vector3& gravity); + /// Return the current time step + decimal getTimeStep() const; + /// Return if the gravity is on bool isGravityEnabled() const; @@ -359,6 +363,14 @@ inline void DynamicsWorld::setGravity(Vector3& gravity) { "Dynamics World: Set gravity vector to " + gravity.to_string()); } +// Return the current time step +/** + * @return The current time step (in seconds) + */ +inline decimal DynamicsWorld::getTimeStep() const { + return mTimeStep; +} + // Return if the gravity is enaled /** * @return True if the gravity is enabled in the world diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index 51c986f3..9c0dbf2e 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -29,16 +29,17 @@ #include "utils/Profiler.h" #include "collision/RaycastInfo.h" #include "memory/MemoryManager.h" -#include "engine/CollisionWorld.h" +#include "engine/DynamicsWorld.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents) + TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents) :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP), mProxyShapesComponents(proxyShapesComponents), mTransformsComponents(transformComponents), + mDynamicsComponents(dynamicsComponents), mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()), mPotentialPairs(collisionDetection.getMemoryManager().getPoolAllocator()), mCollisionDetection(collisionDetection) { @@ -157,6 +158,13 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end startIndex = std::min(startIndex, mProxyShapesComponents.getNbEnabledComponents()); endIndex = std::min(endIndex, mProxyShapesComponents.getNbEnabledComponents()); + // 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 < endIndex; i++) { @@ -166,9 +174,17 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end const Entity& bodyEntity = mProxyShapesComponents.mBodiesEntities[i]; const Transform& transform = mTransformsComponents.getTransform(bodyEntity); - // TODO : Use body linear velocity and compute displacement - const Vector3 displacement = Vector3::zero(); - //const Vector3 displacement = world.mTimeStep * mLinearVelocity; + Vector3 displacement(0, 0, 0); + + // If there is a dynamics component for the current entity + if (mDynamicsComponents.hasComponent(bodyEntity)) { + + // Get the linear velocity from the dynamics component + const Vector3& linearVelocity = mDynamicsComponents.getLinearVelocity(bodyEntity); + + // TODO : Use body linear velocity and compute displacement + displacement = timeStep * linearVelocity; + } // Recompute the world-space AABB of the collision shape AABB aabb; diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index e4ca273a..94047e36 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/src/systems/BroadPhaseSystem.h @@ -32,6 +32,7 @@ #include "containers/Set.h" #include "components/ProxyShapeComponents.h" #include "components/TransformComponents.h" +#include "components/DynamicsComponents.h" /// Namespace ReactPhysics3D namespace reactphysics3d { @@ -148,6 +149,9 @@ class BroadPhaseSystem { /// Reference to the transform components TransformComponents& mTransformsComponents; + /// Reference to the dynamics components + DynamicsComponents& mDynamicsComponents; + /// Set with the broad-phase IDs of all collision shapes that have moved (or have been /// created) during the last simulation step. Those are the shapes that need to be tested /// for overlapping in the next simulation step. @@ -179,7 +183,7 @@ class BroadPhaseSystem { /// Constructor BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents, - TransformComponents &transformComponents); + TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents); /// Destructor ~BroadPhaseSystem() = default;