diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c912f67..640cd580 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_HEADERS "src/systems/ContactSolverSystem.h" "src/systems/DynamicsSystem.h" "src/systems/CollisionDetectionSystem.h" + "src/systems/SolveBallAndSocketJointSystem.h" "src/engine/DynamicsWorld.h" "src/engine/EventListener.h" "src/engine/Island.h" @@ -145,6 +146,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/RigidBodyComponents.h" "src/components/TransformComponents.h" "src/components/ProxyShapeComponents.h" + "src/components/JointComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -223,6 +225,7 @@ SET (REACTPHYSICS3D_SOURCES "src/systems/ContactSolverSystem.cpp" "src/systems/DynamicsSystem.cpp" "src/systems/CollisionDetectionSystem.cpp" + "src/systems/SolveBallAndSocketJointSystem.cpp" "src/engine/DynamicsWorld.cpp" "src/engine/Island.cpp" "src/engine/Material.cpp" @@ -236,6 +239,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/RigidBodyComponents.cpp" "src/components/TransformComponents.cpp" "src/components/ProxyShapeComponents.cpp" + "src/components/JointComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" "src/mathematics/mathematics_functions.cpp" diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 2799758d..29c82c11 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -768,7 +768,7 @@ void RigidBody::setIsSleeping(bool isSleeping) { mWorld.mRigidBodyComponents.setIsSleeping(mEntity, isSleeping); // Notify all the components - mWorld.notifyBodyDisabled(mEntity, isSleeping); + mWorld.setBodyDisabled(mEntity, isSleeping); if (isSleeping) { diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6ab29c5f..ba27a83c 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -63,6 +63,7 @@ class RigidBody : public CollisionBody { Material mMaterial; /// First element of the linked list of joints involving this body + // TODO : Replace this by a list of the joints entities JointListElement* mJointsList; /// True if the center of mass is set by the user @@ -168,9 +169,11 @@ class RigidBody : public CollisionBody { void setAngularDamping(decimal angularDamping); /// Return the first element of the linked list of joints involving this body + // TODO : Remove this const JointListElement* getJointsList() const; /// Return the first element of the linked list of joints involving this body + // TODO : Remove this JointListElement* getJointsList(); /// Apply an external force to the body at its center of mass. diff --git a/src/components/JointComponents.cpp b/src/components/JointComponents.cpp new file mode 100644 index 00000000..a0939294 --- /dev/null +++ b/src/components/JointComponents.cpp @@ -0,0 +1,158 @@ +/******************************************************************************** +* 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 "JointComponents.h" +#include "engine/EntityManager.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +JointComponents::JointComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity)) { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + +// Allocate memory for a given number of components +void JointComponents::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* newJointsEntities = static_cast(newBuffer); + Entity* newBody1Entities = reinterpret_cast(newJointsEntities + nbComponentsToAllocate); + Entity* newBody2Entities = reinterpret_cast(newBody1Entities + nbComponentsToAllocate); + + // If there was already components before + if (mNbComponents > 0) { + + // Copy component data from the previous buffer to the new one + memcpy(newJointsEntities, mJointEntities, mNbComponents * sizeof(Entity)); + memcpy(newBody1Entities, mBody1Entities, mNbComponents * sizeof(Entity)); + memcpy(newBody2Entities, mBody2Entities, mNbComponents * sizeof(Entity)); + + // Deallocate previous memory + mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); + } + + mBuffer = newBuffer; + mNbAllocatedComponents = nbComponentsToAllocate; + mJointEntities = newJointsEntities; + mBody1Entities = newBody1Entities; + mBody2Entities = newBody2Entities; +} + +// Add a component +void JointComponents::addComponent(Entity jointEntity, bool isSleeping, const JointComponent& component) { + + // Prepare to add new component (allocate memory if necessary and compute insertion index) + uint32 index = prepareAddComponent(isSleeping); + + // Insert the new component data + new (mJointEntities + index) Entity(jointEntity); + new (mBody1Entities + index) Entity(component.body1Entity); + new (mBody2Entities + index) Entity(component.body2Entity); + + // Map the entity with the new component lookup index + mMapEntityToComponentIndex.add(Pair(jointEntity, 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 JointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { + + const Entity entity = mJointEntities[srcIndex]; + + // Copy the data of the source component to the destination location + new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]); + new (mBody1Entities + destIndex) Entity(mBody1Entities[srcIndex]); + new (mBody2Entities + destIndex) Entity(mBody2Entities[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[mJointEntities[destIndex]] == destIndex); +} + +// Swap two components in the array +void JointComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity jointEntity1(mJointEntities[index1]); + Entity body1Entity1(mBody1Entities[index1]); + Entity body2Entity1(mBody2Entities[index1]); + + // Destroy component 1 + destroyComponent(index1); + + moveComponentToIndex(index2, index1); + + // Reconstruct component 1 at component 2 location + new (mJointEntities + index2) Entity(jointEntity1); + new (mBody1Entities + index2) Entity(body1Entity1); + new (mBody2Entities + index2) Entity(body2Entity1); + + // Update the entity to component index mapping + mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); + + assert(mMapEntityToComponentIndex[mJointEntities[index1]] == index1); + assert(mMapEntityToComponentIndex[mJointEntities[index2]] == index2); + assert(mNbComponents == static_cast(mMapEntityToComponentIndex.size())); +} + +// Destroy a component at a given index +void JointComponents::destroyComponent(uint32 index) { + + Components::destroyComponent(index); + + assert(mMapEntityToComponentIndex[mJointEntities[index]] == index); + + mMapEntityToComponentIndex.remove(mJointEntities[index]); + + mJointEntities[index].~Entity(); + mBody1Entities[index].~Entity(); + mBody2Entities[index].~Entity(); +} diff --git a/src/components/JointComponents.h b/src/components/JointComponents.h new file mode 100644 index 00000000..f0deb297 --- /dev/null +++ b/src/components/JointComponents.h @@ -0,0 +1,127 @@ +/******************************************************************************** +* 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_JOINT_COMPONENTS_H +#define REACTPHYSICS3D_JOINT_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 JointComponents +/** + * This class represent the component of the ECS that contains generic information about + * all the joints. + */ +class JointComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of joint entities + Entity* mJointEntities; + + /// Array of body entities of the first bodies of the joints + Entity* mBody1Entities; + + /// Array of body entities of the first bodies of the joints + Entity* mBody2Entities; + + // -------------------- 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 JointComponent { + + const Entity body1Entity; + const Entity body2Entity; + + /// Constructor + JointComponent(Entity body1Entity, Entity body2Entity) + : body1Entity(body1Entity), body2Entity(body2Entity) { + + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + JointComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~JointComponents() override = default; + + /// Add a component + void addComponent(Entity jointEntity, bool isSleeping, const JointComponent& component); + + /// Return the entity of the first body of a joint + Entity getBody1Entity(Entity jointEntity) const; + + /// Return the entity of the second body of a joint + Entity getBody2Entity(Entity jointEntity) const; + + // -------------------- Friendship -------------------- // + + friend class BroadPhaseSystem; +}; + +// Return the entity of the first body of a joint +inline Entity JointComponents::getBody1Entity(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBody1Entities[mMapEntityToComponentIndex[jointEntity]]; +} + +// Return the entity of the second body of a joint +inline Entity JointComponents::getBody2Entity(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBody2Entities[mMapEntityToComponentIndex[jointEntity]]; +} + +} + +#endif diff --git a/src/components/TransformComponents.h b/src/components/TransformComponents.h index f50c52b5..5ca00332 100644 --- a/src/components/TransformComponents.h +++ b/src/components/TransformComponents.h @@ -108,11 +108,13 @@ class TransformComponents : public Components { // Return the transform of an entity inline Transform& TransformComponents::getTransform(Entity bodyEntity) const { + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); return mTransforms[mMapEntityToComponentIndex[bodyEntity]]; } // Set the transform of an entity inline void TransformComponents::setTransform(Entity bodyEntity, const Transform& transform) { + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); mTransforms[mMapEntityToComponentIndex[bodyEntity]] = transform; } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index a5b100a2..1d544a2c 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -34,8 +34,8 @@ using namespace reactphysics3d; const decimal BallAndSocketJoint::BETA = decimal(0.2); // Constructor -BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jointInfo) - : Joint(id, jointInfo), mImpulse(Vector3(0, 0, 0)) { +BallAndSocketJoint::BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo) + : Joint(entity, jointInfo), mImpulse(Vector3(0, 0, 0)) { // Compute the local-space anchor point for each body mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index ab8af193..64e12c36 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -125,7 +125,7 @@ class BallAndSocketJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jointInfo); + BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo); /// Destructor virtual ~BallAndSocketJoint() override = default; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index d317d237..6ec86b9e 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -34,8 +34,8 @@ using namespace reactphysics3d; const decimal FixedJoint::BETA = decimal(0.2); // Constructor -FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo) - : Joint(id, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { +FixedJoint::FixedJoint(Entity entity, const FixedJointInfo& jointInfo) + : Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body const Transform& transform1 = mBody1->getTransform(); diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index 0fa0f2fc..ce919eb1 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -136,7 +136,7 @@ class FixedJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - FixedJoint(uint id, const FixedJointInfo& jointInfo); + FixedJoint(Entity entity, const FixedJointInfo& jointInfo); /// Destructor virtual ~FixedJoint() override = default; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 5ae7755a..4e72b108 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -34,8 +34,8 @@ using namespace reactphysics3d; const decimal HingeJoint::BETA = decimal(0.2); // Constructor -HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo) - : Joint(id, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), +HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo) + : Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index 183027a8..191ff1df 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -287,7 +287,7 @@ class HingeJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - HingeJoint(uint id, const HingeJointInfo& jointInfo); + HingeJoint(Entity entity, const HingeJointInfo& jointInfo); /// Destructor virtual ~HingeJoint() override = default; diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp index f1ca34eb..1d7c9b6c 100644 --- a/src/constraint/Joint.cpp +++ b/src/constraint/Joint.cpp @@ -29,8 +29,8 @@ using namespace reactphysics3d; // Constructor -Joint::Joint(uint id, const JointInfo& jointInfo) - :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()), +Joint::Joint(Entity entity, const JointInfo& jointInfo) + :mEntity(entity), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()), mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type), mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index e21f0263..54b7eb8a 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -120,8 +120,8 @@ class Joint { // -------------------- Attributes -------------------- // - /// Id of the joint - uint mId; + /// Entity ID of the joint + Entity mEntity; /// Pointer to the first body of the joint // TODO : Use Entities instead @@ -177,7 +177,7 @@ class Joint { // -------------------- Methods -------------------- // /// Constructor - Joint(uint id, const JointInfo& jointInfo); + Joint(Entity entity, const JointInfo& jointInfo); /// Destructor virtual ~Joint() = default; @@ -194,17 +194,14 @@ class Joint { /// Return the reference to the body 2 RigidBody* getBody2() const; - /// Return true if the constraint is active - bool isActive() const; - /// Return the type of the constraint JointType getType() const; /// Return true if the collision between the two bodies of the joint is enabled bool isCollisionEnabled() const; - /// Return the id of the joint - uint getId() const; + /// Return the entity id of the joint + Entity getEntity() const; /// Return a string representation virtual std::string to_string() const=0; @@ -232,14 +229,6 @@ inline RigidBody* Joint::getBody2() const { return mBody2; } -// Return true if the joint is active -/** - * @return True if the joint is active - */ -inline bool Joint::isActive() const { - return (mBody1->isActive() && mBody2->isActive()); -} - // Return the type of the joint /** * @return The type of the joint @@ -257,12 +246,12 @@ inline bool Joint::isCollisionEnabled() const { return mIsCollisionEnabled; } -// Return the id of the joint +// Return the entity id of the joint /** - * @return The id of the joint + * @return The entity of the joint */ -inline uint Joint::getId() const { - return mId; +inline Entity Joint::getEntity() const { + return mEntity; } // Return true if the joint has already been added into an island diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 86864d45..dafb3c48 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -34,8 +34,8 @@ using namespace reactphysics3d; const decimal SliderJoint::BETA = decimal(0.2); // Constructor -SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo) - : Joint(id, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), +SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo) + : Joint(entity, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minTranslationLimit), diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index b9a83de0..e0374eb5 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -289,7 +289,7 @@ class SliderJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - SliderJoint(uint id, const SliderJointInfo& jointInfo); + SliderJoint(Entity entity, const SliderJointInfo& jointInfo); /// Destructor virtual ~SliderJoint() override = default; diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 73266988..33b11b48 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -40,6 +40,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge : mConfig(worldSettings), mEntityManager(mMemoryManager.getPoolAllocator()), mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()), + mJointsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), @@ -207,7 +208,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { } // Notify the world if a body is disabled (sleeping) or not -void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) { +void CollisionWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { if (isDisabled == mCollisionBodyComponents.getIsEntityDisabled(bodyEntity)) return; @@ -228,6 +229,38 @@ void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) { mProxyShapesComponents.setIsEntityDisabled(proxyShapesEntities[i], isDisabled); } + + // Disable the joints of the body if necessary + if (mRigidBodyComponents.hasComponent(bodyEntity)) { + + RigidBody* body = mRigidBodyComponents.getRigidBody(bodyEntity); + + // For each joint of the body + for(JointListElement* jointElem = body->getJointsList(); jointElem != nullptr; jointElem = jointElem->next) { + + // If both bodies of the joint are disabled + if (mRigidBodyComponents.getIsEntityDisabled(jointElem->joint->getBody1()->getEntity()) && + mRigidBodyComponents.getIsEntityDisabled(jointElem->joint->getBody2()->getEntity())) { + + // We disable the joint + setJointDisabled(jointElem->joint->getEntity(), true); + } + else { + + // Enable the joint + setJointDisabled(jointElem->joint->getEntity(), false); + } + } + } +} + +// Notify the world whether a joint is disabled or not +void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) { + + if (isDisabled == mJointsComponents.getIsEntityDisabled(jointEntity)) return; + + // TODO : Make sure we notify all the components here ... + mJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); } // Return true if two bodies overlap diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 0541a4cd..c74e8580 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -37,6 +37,7 @@ #include "components/RigidBodyComponents.h" #include "components/TransformComponents.h" #include "components/ProxyShapeComponents.h" +#include "components/JointComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -88,6 +89,9 @@ class CollisionWorld { /// Proxy-Shapes Components ProxyShapeComponents mProxyShapesComponents; + /// Joint Components + JointComponents mJointsComponents; + /// Reference to the collision detection CollisionDetectionSystem mCollisionDetection; @@ -124,7 +128,10 @@ class CollisionWorld { // -------------------- Methods -------------------- // /// Notify the world if a body is disabled (slepping or inactive) or not - void notifyBodyDisabled(Entity entity, bool isDisabled); + void setBodyDisabled(Entity entity, bool isDisabled); + + /// Notify the world whether a joint is disabled or not + void setJointDisabled(Entity jointEntity, bool isDisabled); public : diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index adcc1a60..06a3896d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -59,8 +59,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), - mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), - mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { + mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mCurrentJointId(0) { #ifdef IS_PROFILING_ACTIVE @@ -318,10 +317,15 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { */ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { + // Create a new entity for the joint + Entity entity = mEntityManager.createEntity(); + Joint* newJoint = nullptr; - // Get the next available joint ID - uint jointId = computeNextAvailableJointId(); + bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && + mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); + JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity()); + mJointsComponents.addComponent(entity, isJointDisabled, jointComponent); // Allocate memory to create the new joint switch(jointInfo.type) { @@ -333,7 +337,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); - newJoint = new (allocatedMemory) BallAndSocketJoint(jointId, info); + newJoint = new (allocatedMemory) BallAndSocketJoint(entity, info); break; } @@ -343,7 +347,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) SliderJoint(jointId, info); + newJoint = new (allocatedMemory) SliderJoint(entity, info); break; } @@ -353,7 +357,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) HingeJoint(jointId, info); + newJoint = new (allocatedMemory) HingeJoint(entity, info); break; } @@ -363,7 +367,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) FixedJoint(jointId, info); + newJoint = new (allocatedMemory) FixedJoint(entity, info); break; } @@ -385,9 +389,9 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { mJoints.add(newJoint); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, - "Joint " + std::to_string(newJoint->getId()) + ": New joint created"); + "Joint " + std::to_string(newJoint->getEntity().id) + ": New joint created"); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, - "Joint " + std::to_string(newJoint->getId()) + ": " + newJoint->to_string()); + "Joint " + std::to_string(newJoint->getEntity().id) + ": " + newJoint->to_string()); // Add the joint into the joint list of the bodies involved in the joint addJointToBody(newJoint); @@ -405,7 +409,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { assert(joint != nullptr); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint, - "Joint " + std::to_string(joint->getId()) + ": joint destroyed"); + "Joint " + std::to_string(joint->getEntity().id) + ": joint destroyed"); // If the collision between the two bodies of the constraint was disabled if (!joint->isCollisionEnabled()) { @@ -427,15 +431,14 @@ void DynamicsWorld::destroyJoint(Joint* joint) { size_t nbBytes = joint->getSizeInBytes(); - // Add the joint ID to the list of free IDs - mFreeJointsIDs.add(joint->getId()); + // Destroy the corresponding entity and its components + // TODO : Make sure we remove all its components here + mJointsComponents.removeComponent(joint->getEntity()); + mEntityManager.destroyEntity(joint->getEntity()); // Call the destructor of the joint joint->~Joint(); - // Add the joint ID to the list of free IDs - mFreeJointsIDs.add(joint->getId()); - // Release the allocated memory mMemoryManager.release(MemoryManager::AllocationType::Pool, joint, nbBytes); } @@ -453,7 +456,7 @@ void DynamicsWorld::addJointToBody(Joint* joint) { joint->mBody1->mJointsList = jointListElement1; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(joint->mBody1->getEntity().id) + ": Joint " + std::to_string(joint->getId()) + + "Body " + std::to_string(joint->mBody1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + " added to body"); // Add the joint at the beginning of the linked list of joints of the second body @@ -464,27 +467,10 @@ void DynamicsWorld::addJointToBody(Joint* joint) { joint->mBody2->mJointsList = jointListElement2; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(joint->mBody2->getEntity().id) + ": Joint " + std::to_string(joint->getId()) + + "Body " + std::to_string(joint->mBody2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + " added to body"); } -// Return the next available joint Id -uint DynamicsWorld::computeNextAvailableJointId() { - - // Compute the joint ID - uint jointId; - if (mFreeJointsIDs.size() != 0) { - jointId = mFreeJointsIDs[mFreeJointsIDs.size() - 1]; - mFreeJointsIDs.removeAt(mFreeJointsIDs.size() - 1); - } - else { - jointId = mCurrentJointId; - mCurrentJointId++; - } - - return jointId; -} - // Compute the islands using potential contacts and joints /// We compute the islands before creating the actual contacts here because we want all /// the contact manifolds and contact points of the same island diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index edbd63ef..2c6913da 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -98,9 +98,6 @@ class DynamicsWorld : public CollisionWorld { /// becomes smaller than the sleep velocity. decimal mTimeBeforeSleep; - /// List of free ID for joints - List mFreeJointsIDs; - /// Current joint id uint mCurrentJointId; @@ -124,9 +121,6 @@ class DynamicsWorld : public CollisionWorld { /// Add the joint to the list of joints of the two bodies involved in the joint void addJointToBody(Joint* joint); - /// Return the next available joint id - uint computeNextAvailableJointId(); - public : // -------------------- Methods -------------------- // diff --git a/src/systems/ConstraintSolverSystem.cpp b/src/systems/ConstraintSolverSystem.cpp index 0afe523a..ae637ba7 100644 --- a/src/systems/ConstraintSolverSystem.cpp +++ b/src/systems/ConstraintSolverSystem.cpp @@ -32,7 +32,8 @@ using namespace reactphysics3d; // Constructor ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents) - : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents) { + : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents), + mSolveBallAndSocketJointSystem(rigidBodyComponents) { #ifdef IS_PROFILING_ACTIVE diff --git a/src/systems/ConstraintSolverSystem.h b/src/systems/ConstraintSolverSystem.h index 6898f544..da7ec7ec 100644 --- a/src/systems/ConstraintSolverSystem.h +++ b/src/systems/ConstraintSolverSystem.h @@ -30,6 +30,7 @@ #include "configuration.h" #include "mathematics/mathematics.h" #include "engine/Islands.h" +#include "systems/SolveBallAndSocketJointSystem.h" namespace reactphysics3d { @@ -153,6 +154,9 @@ class ConstraintSolverSystem { /// Constraint solver data used to initialize and solve the constraints ConstraintSolverData mConstraintSolverData; + /// Solver for the BallAndSocketJoint constraints + SolveBallAndSocketJointSystem mSolveBallAndSocketJointSystem; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -198,6 +202,7 @@ class ConstraintSolverSystem { // Set the profiler inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) { mProfiler = profiler; + mSolveBallAndSocketJointSystem.setProfiler(profiler); } #endif diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp new file mode 100644 index 00000000..76166cf2 --- /dev/null +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -0,0 +1,35 @@ +/******************************************************************************** +* 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/SolveBallAndSocketJointSystem.h" + +using namespace reactphysics3d; + +// Constructor +SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents) + :mRigidBodyComponents(rigidBodyComponents) { + +} diff --git a/src/systems/SolveBallAndSocketJointSystem.h b/src/systems/SolveBallAndSocketJointSystem.h new file mode 100644 index 00000000..b77fee1a --- /dev/null +++ b/src/systems/SolveBallAndSocketJointSystem.h @@ -0,0 +1,85 @@ +/******************************************************************************** +* 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_SOLVE_BALL_SOCKET_JOINT_SYSTEM_H +#define REACTPHYSICS3D_SOLVE_BALL_SOCKET_JOINT_SYSTEM_H + +// Libraries +#include "utils/Profiler.h" +#include "components/RigidBodyComponents.h" +#include "components/TransformComponents.h" + +namespace reactphysics3d { + +// Class SolveBallAndSocketJointSystem +/** + * This class is responsible to solve the BallAndSocketJoint constraints + */ +class SolveBallAndSocketJointSystem { + + private : + + // -------------------- Attributes -------------------- // + + /// Reference to the rigid body components + RigidBodyComponents& mRigidBodyComponents; + +#ifdef IS_PROFILING_ACTIVE + + /// Pointer to the profiler + Profiler* mProfiler; +#endif + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents); + + /// Destructor + ~SolveBallAndSocketJointSystem() = default; + +#ifdef IS_PROFILING_ACTIVE + + /// Set the profiler + void setProfiler(Profiler* profiler); + +#endif + +}; + +#ifdef IS_PROFILING_ACTIVE + +// Set the profiler +inline void SolveBallAndSocketJointSystem::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +#endif + +} + +#endif