diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index b0fba2e1..98843732 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -86,8 +86,8 @@ void RigidBody::setType(BodyType type) { if (type == BodyType::STATIC) { // Reset the velocity to zero - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero()); - mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, Vector3::zero()); } // If it is a static or a kinematic body @@ -295,11 +295,11 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal); // Update the linear velocity of the center of mass - Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); - const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + Vector3 linearVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); + const Vector3& angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string()); @@ -536,7 +536,7 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) { if (mWorld.mRigidBodyComponents.getBodyType(mEntity) == BodyType::STATIC) return; // Update the linear velocity of the current body state - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); // If the linear velocity is not zero, awake the body if (linearVelocity.lengthSquare() > decimal(0.0)) { @@ -559,7 +559,7 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { if (mWorld.mRigidBodyComponents.getBodyType(mEntity) == BodyType::STATIC) return; // Set the angular velocity - mWorld.mDynamicsComponents.setAngularVelocity(mEntity, angularVelocity); + mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, angularVelocity); // If the velocity is not zero, awake the body if (angularVelocity.lengthSquare() > decimal(0.0)) { @@ -584,11 +584,11 @@ void RigidBody::setTransform(const Transform& transform) { mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal); // Update the linear velocity of the center of mass - Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); - const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); + const Vector3& angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); CollisionBody::setTransform(transform); @@ -695,10 +695,10 @@ void RigidBody::recomputeMassInformation() { updateInertiaTensorInverseWorld(); // Update the linear velocity of the center of mass - Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); - Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); + Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass); - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, linearVelocity); } // Return the linear velocity @@ -706,7 +706,7 @@ void RigidBody::recomputeMassInformation() { * @return The linear velocity vector of the body */ Vector3 RigidBody::getLinearVelocity() const { - return mWorld.mDynamicsComponents.getLinearVelocity(mEntity); + return mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); } // Return the angular velocity of the body @@ -714,7 +714,7 @@ Vector3 RigidBody::getLinearVelocity() const { * @return The angular velocity vector of the body */ Vector3 RigidBody::getAngularVelocity() const { - return mWorld.mDynamicsComponents.getAngularVelocity(mEntity); + return mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); } // Return true if the gravity needs to be applied to this rigid body @@ -777,8 +777,8 @@ void RigidBody::setIsSleeping(bool isSleeping) { if (isSleeping) { - mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero()); - mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setLinearVelocity(mEntity, Vector3::zero()); + mWorld.mRigidBodyComponents.setAngularVelocity(mEntity, Vector3::zero()); mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero()); mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero()); } diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 490562ce..9c16ac02 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -53,11 +53,11 @@ using namespace std; // Constructor CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents, - DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager) + RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mOverlappingPairs(mMemoryManager.getPoolAllocator()), - mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, dynamicsComponents), + mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, rigidBodyComponents), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 85930a78..dd91b296 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -270,7 +270,7 @@ class CollisionDetection { /// Constructor CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents, + TransformComponents& transformComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager); /// Destructor diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index a0705531..de9c78f5 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -32,11 +32,10 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; - // Constructor DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) + sizeof(bool)) { @@ -59,9 +58,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { // New pointers to components data Entity* newBodies = static_cast(newBuffer); - Vector3* newLinearVelocities = reinterpret_cast(newBodies + nbComponentsToAllocate); - Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newBodies + nbComponentsToAllocate); Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); @@ -85,8 +82,6 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { // Copy component data from the previous buffer to the new one memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity)); - memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3)); - memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); @@ -112,8 +107,6 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mBuffer = newBuffer; mBodies = newBodies; - mLinearVelocities = newLinearVelocities; - mAngularVelocities = newAngularVelocities; mConstrainedLinearVelocities = newConstrainedLinearVelocities; mConstrainedAngularVelocities = newConstrainedAngularVelocities; mSplitLinearVelocities = newSplitLinearVelocities; @@ -144,8 +137,6 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const // Insert the new component data new (mBodies + index) Entity(bodyEntity); - new (mLinearVelocities + index) Vector3(0, 0, 0); - new (mAngularVelocities + index) Vector3(0, 0, 0); new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); new (mSplitLinearVelocities + index) Vector3(0, 0, 0); @@ -182,8 +173,6 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) // 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]); new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); @@ -222,8 +211,6 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { // Copy component 1 data Entity entity1(mBodies[index1]); - Vector3 linearVelocity1(mLinearVelocities[index1]); - Vector3 angularVelocity1(mAngularVelocities[index1]); Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); @@ -250,8 +237,6 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { // Reconstruct component 1 at component 2 location new (mBodies + index2) Entity(entity1); - new (mLinearVelocities + index2) Vector3(linearVelocity1); - new (mAngularVelocities + index2) Vector3(angularVelocity1); new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); @@ -289,8 +274,6 @@ void DynamicsComponents::destroyComponent(uint32 index) { mMapEntityToComponentIndex.remove(mBodies[index]); mBodies[index].~Entity(); - mLinearVelocities[index].~Vector3(); - mAngularVelocities[index].~Vector3(); mConstrainedLinearVelocities[index].~Vector3(); mConstrainedAngularVelocities[index].~Vector3(); mSplitLinearVelocities[index].~Vector3(); diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index f19df426..820d4c44 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -40,17 +40,6 @@ namespace reactphysics3d { class MemoryAllocator; class EntityManager; -/// Enumeration for the type of a body -/// STATIC : A static body has infinite mass, zero velocity but the position can be -/// changed manually. A static body does not collide with other static or kinematic bodies. -/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its -/// position is computed by the physics engine. A kinematic body does not collide with -/// other static or kinematic bodies. -/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its -/// position is determined by the physics engine. A dynamic body can collide with other -/// dynamic, static or kinematic bodies. -enum class BodyType {STATIC, KINEMATIC, DYNAMIC}; - // Class DynamicsComponents /** * This class represent the component of the ECS that contains the variables concerning dynamics @@ -66,12 +55,6 @@ class DynamicsComponents : public Components { /// 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; - /// Array with the constrained linear velocity of each component Vector3* mConstrainedLinearVelocities; @@ -165,12 +148,6 @@ class DynamicsComponents : public Components { /// Add a component void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component); - /// Return the linear velocity of an entity - const Vector3& getLinearVelocity(Entity bodyEntity) const; - - /// Return the angular velocity of an entity - const Vector3& getAngularVelocity(Entity bodyEntity) const; - /// Return the constrained linear velocity of an entity const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; @@ -225,12 +202,6 @@ class DynamicsComponents : public Components { /// Return true if the entity is already in an island bool getIsAlreadyInIsland(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); - /// Set the constrained linear velocity of an entity void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity); @@ -296,38 +267,6 @@ class DynamicsComponents : public Components { friend class SliderJoint; }; -// Return the linear velocity of an entity -inline const Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; -} - -// Return the angular velocity of an entity -inline const 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; -} - // Return the constrained linear velocity of an entity inline const Vector3& DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index b71b3ba2..dfa2fdbf 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -36,7 +36,8 @@ using namespace reactphysics3d; // Constructor RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(RigidBody*) + - sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(BodyType)) { + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(BodyType) + + sizeof(Vector3) + sizeof(Vector3)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -61,6 +62,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { bool* newIsSleeping = reinterpret_cast(newIsAllowedToSleep + nbComponentsToAllocate); decimal* newSleepTimes = reinterpret_cast(newIsSleeping + nbComponentsToAllocate); BodyType* newBodyTypes = reinterpret_cast(newSleepTimes + nbComponentsToAllocate); + Vector3* newLinearVelocities = reinterpret_cast(newBodyTypes + nbComponentsToAllocate); + Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -72,6 +75,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newIsSleeping, mIsSleeping, mNbComponents * sizeof(bool)); memcpy(newSleepTimes, mSleepTimes, mNbComponents * sizeof(bool)); memcpy(newBodyTypes, mBodyTypes, mNbComponents * sizeof(BodyType)); + memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -85,6 +90,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mSleepTimes = newSleepTimes; mNbAllocatedComponents = nbComponentsToAllocate; mBodyTypes = newBodyTypes; + mLinearVelocities = newLinearVelocities; + mAngularVelocities = newAngularVelocities; } // Add a component @@ -100,6 +107,8 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mIsSleeping[index] = false; mSleepTimes[index] = decimal(0); mBodyTypes[index] = component.bodyType; + new (mLinearVelocities + index) Vector3(0, 0, 0); + new (mAngularVelocities + index) Vector3(0, 0, 0); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -123,6 +132,8 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mIsSleeping[destIndex] = mIsSleeping[srcIndex]; mSleepTimes[destIndex] = mSleepTimes[srcIndex]; mBodyTypes[destIndex] = mBodyTypes[srcIndex]; + new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]); + new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -145,6 +156,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { bool isSleeping1 = mIsSleeping[index1]; decimal sleepTime1 = mSleepTimes[index1]; BodyType bodyType1 = mBodyTypes[index1]; + Vector3 linearVelocity1(mLinearVelocities[index1]); + Vector3 angularVelocity1(mAngularVelocities[index1]); // Destroy component 1 destroyComponent(index1); @@ -158,6 +171,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mIsSleeping[index2] = isSleeping1; mSleepTimes[index2] = sleepTime1; mBodyTypes[index2] = bodyType1; + new (mLinearVelocities + index2) Vector3(linearVelocity1); + new (mAngularVelocities + index2) Vector3(angularVelocity1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -178,4 +193,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mBodiesEntities[index].~Entity(); mRigidBodies[index] = nullptr; + mLinearVelocities[index].~Vector3(); + mAngularVelocities[index].~Vector3(); } diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index df4641f2..1a69fcb8 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -41,6 +41,17 @@ class EntityManager; class RigidBody; enum class BodyType; +/// Enumeration for the type of a body +/// STATIC : A static body has infinite mass, zero velocity but the position can be +/// changed manually. A static body does not collide with other static or kinematic bodies. +/// KINEMATIC : A kinematic body has infinite mass, the velocity can be changed manually and its +/// position is computed by the physics engine. A kinematic body does not collide with +/// other static or kinematic bodies. +/// DYNAMIC : A dynamic body has non-zero mass, non-zero velocity determined by forces and its +/// position is determined by the physics engine. A dynamic body can collide with other +/// dynamic, static or kinematic bodies. +enum class BodyType {STATIC, KINEMATIC, DYNAMIC}; + // Class RigidBodyComponents /** * This class represent the component of the ECS that contains data about a rigid body. @@ -70,6 +81,12 @@ class RigidBodyComponents : public Components { /// Array with the type of bodies (static, kinematic or dynamic) BodyType* mBodyTypes; + /// 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 @@ -136,6 +153,17 @@ class RigidBodyComponents : public Components { /// Set the body type of a body void setBodyType(Entity bodyEntity, BodyType bodyType); + /// Return the linear velocity of an entity + const Vector3& getLinearVelocity(Entity bodyEntity) const; + + /// Set the linear velocity of an entity + void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity); + + /// Return the angular velocity of an entity + const Vector3& getAngularVelocity(Entity bodyEntity) const; + + /// Set the angular velocity of an entity + void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity); }; // Return a pointer to a body rigid @@ -210,6 +238,38 @@ inline void RigidBodyComponents::setBodyType(Entity bodyEntity, BodyType bodyTyp mBodyTypes[mMapEntityToComponentIndex[bodyEntity]] = bodyType; } +// Return the linear velocity of an entity +inline const Vector3& RigidBodyComponents::getLinearVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the angular velocity of an entity +inline const Vector3& RigidBodyComponents::getAngularVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the linear velocity of an entity +inline void RigidBodyComponents::setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = linearVelocity; +} + +// Set the angular velocity of an entity +inline void RigidBodyComponents::setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity; +} + } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index b795560d..40c21b9a 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -27,6 +27,7 @@ #include "BallAndSocketJoint.h" #include "engine/ConstraintSolver.h" #include "components/DynamicsComponents.h" +#include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index a195e974..c67b9862 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -27,6 +27,7 @@ #include "FixedJoint.h" #include "engine/ConstraintSolver.h" #include "components/DynamicsComponents.h" +#include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index a1651027..29f29879 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -27,6 +27,7 @@ #include "HingeJoint.h" #include "engine/ConstraintSolver.h" #include "components/DynamicsComponents.h" +#include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 48be0755..e782d489 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -27,6 +27,7 @@ #include "SliderJoint.h" #include "engine/ConstraintSolver.h" #include "components/DynamicsComponents.h" +#include "components/RigidBodyComponents.h" using namespace reactphysics3d; diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 5bc953e5..fdd15bf4 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -41,7 +41,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mDynamicsComponents(mMemoryManager.getBaseAllocator()), - mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mDynamicsComponents, mMemoryManager), + mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), mIsLoggerCreatedByUser(logger != nullptr) { diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index d4f1e45f..acb7a8b2 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -44,12 +44,13 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, DynamicsComponents& dynamicsComponents, +ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, + RigidBodyComponents& rigidBodyComponents, DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings) :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr), mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents), - mDynamicsComponents(dynamicsComponents), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), - mWorldSettings(worldSettings) { + mRigidBodyComponents(rigidBodyComponents), mDynamicsComponents(dynamicsComponents), + mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mWorldSettings(worldSettings) { #ifdef IS_PROFILING_ACTIVE mProfiler = nullptr; @@ -152,10 +153,10 @@ void ContactSolver::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero(); // Get the velocities of the bodies - const Vector3& v1 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity1); - const Vector3& w1 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity1); - const Vector3& v2 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity2); - const Vector3& w2 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity2); + const Vector3& v1 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity1); + const Vector3& w1 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity1); + const Vector3& v2 = mRigidBodyComponents.getLinearVelocity(externalManifold.bodyEntity2); + const Vector3& w2 = mRigidBodyComponents.getAngularVelocity(externalManifold.bodyEntity2); // For each contact point of the contact manifold assert(externalManifold.getNbContactPoints() > 0); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 245dac07..e50a4684 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -45,6 +45,7 @@ class Island; class RigidBody; class CollisionBodyComponents; class DynamicsComponents; +class RigidBodyComponents; class ProxyShapeComponents; // Class Contact Solver @@ -312,6 +313,9 @@ class ContactSolver { /// Reference to the body components CollisionBodyComponents& mBodyComponents; + /// Reference to the dynamics components + RigidBodyComponents& mRigidBodyComponents; + /// Reference to the dynamics components DynamicsComponents& mDynamicsComponents; @@ -360,8 +364,8 @@ class ContactSolver { /// Constructor ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, - DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents, - const WorldSettings& worldSettings); + RigidBodyComponents& rigidBodyComponents, DynamicsComponents &dynamicsComponents, + ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings); /// Destructor ~ContactSolver() = default; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 71cb745b..29ed2f6d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,7 +50,8 @@ 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, mDynamicsComponents, mProxyShapesComponents, mConfig), + mContactSolver(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mDynamicsComponents, + mProxyShapesComponents, mConfig), mConstraintSolver(mIslands, mDynamicsComponents), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), @@ -200,8 +201,8 @@ void DynamicsWorld::updateBodiesState() { for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { // Update the linear and angular velocity of the body - mDynamicsComponents.mLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i]; - mDynamicsComponents.mAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i]; + mRigidBodyComponents.setLinearVelocity(mDynamicsComponents.mBodies[i], mDynamicsComponents.mConstrainedLinearVelocities[i]); + mRigidBodyComponents.setAngularVelocity(mDynamicsComponents.mBodies[i], mDynamicsComponents.mConstrainedAngularVelocities[i]); // Update the position of the center of mass of the body mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i]; @@ -259,11 +260,14 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0)); assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0)); + const Vector3& linearVelocity = mRigidBodyComponents.getLinearVelocity(mDynamicsComponents.mBodies[i]); + const Vector3& angularVelocity = mRigidBodyComponents.getAngularVelocity(mDynamicsComponents.mBodies[i]); + // Integrate the external force to get the new velocity of the body - mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep * + mDynamicsComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep * mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i]; - mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] + - mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i]; + mDynamicsComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep * + mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i]; } // Apply gravity force @@ -805,26 +809,24 @@ void DynamicsWorld::updateSleepingBodies() { const Entity bodyEntity = mIslands.bodyEntities[i][b]; - RigidBody* body = mRigidBodyComponents.getRigidBody(bodyEntity); - // Skip static bodies if (mRigidBodyComponents.getBodyType(bodyEntity) == BodyType::STATIC) continue; // If the body is velocity is large enough to stay awake - if (mDynamicsComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare || - mDynamicsComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare || - !mRigidBodyComponents.getIsAllowedToSleep(body->getEntity())) { + if (mRigidBodyComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare || + mRigidBodyComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare || + !mRigidBodyComponents.getIsAllowedToSleep(bodyEntity)) { // Reset the sleep time of the body - mRigidBodyComponents.setSleepTime(body->getEntity(), decimal(0.0)); + mRigidBodyComponents.setSleepTime(bodyEntity, decimal(0.0)); minSleepTime = decimal(0.0); } else { // If the body velocity is below the sleeping velocity threshold // Increase the sleep time - decimal sleepTime = mRigidBodyComponents.getSleepTime(body->getEntity()); - mRigidBodyComponents.setSleepTime(body->getEntity(), sleepTime + mTimeStep); - sleepTime = mRigidBodyComponents.getSleepTime(body->getEntity()); + decimal sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); + mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + mTimeStep); + sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity); if (sleepTime < minSleepTime) { minSleepTime = sleepTime; } diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index 3b7ec9dc..e9374b0a 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -36,11 +36,10 @@ using namespace reactphysics3d; // Constructor BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents) + TransformComponents& transformComponents, RigidBodyComponents &rigidBodyComponents) :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP), mProxyShapesComponents(proxyShapesComponents), mTransformsComponents(transformComponents), - mDynamicsComponents(dynamicsComponents), - mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()), + mRigidBodyComponents(rigidBodyComponents), mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()), mCollisionDetection(collisionDetection) { #ifdef IS_PROFILING_ACTIVE @@ -175,10 +174,10 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end // If there is a dynamics component for the current entity Vector3 displacement(0, 0, 0); - if (mDynamicsComponents.hasComponent(bodyEntity)) { + if (mRigidBodyComponents.hasComponent(bodyEntity)) { // Get the linear velocity from the dynamics component - const Vector3& linearVelocity = mDynamicsComponents.getLinearVelocity(bodyEntity); + const Vector3& linearVelocity = mRigidBodyComponents.getLinearVelocity(bodyEntity); // TODO : Use body linear velocity and compute displacement displacement = timeStep * linearVelocity; diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index 7931da92..abcc7490 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/src/systems/BroadPhaseSystem.h @@ -32,7 +32,7 @@ #include "containers/Set.h" #include "components/ProxyShapeComponents.h" #include "components/TransformComponents.h" -#include "components/DynamicsComponents.h" +#include "components/RigidBodyComponents.h" #include /// Namespace ReactPhysics3D @@ -120,8 +120,8 @@ class BroadPhaseSystem { /// Reference to the transform components TransformComponents& mTransformsComponents; - /// Reference to the dynamics components - DynamicsComponents& mDynamicsComponents; + /// Reference to the rigid body components + RigidBodyComponents& mRigidBodyComponents; /// 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 @@ -151,7 +151,7 @@ class BroadPhaseSystem { /// Constructor BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents); + TransformComponents& transformComponents, RigidBodyComponents& rigidBodyComponents); /// Destructor ~BroadPhaseSystem() = default;