From f29810334e2e7ac10450299295cdd981ea47433f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 24 Sep 2019 17:45:43 +0200 Subject: [PATCH] Working on JointComponents --- src/body/RigidBody.cpp | 37 +-------------- src/body/RigidBody.h | 25 +--------- src/components/CollisionBodyComponents.h | 4 +- src/components/JointComponents.cpp | 10 +++- src/components/JointComponents.h | 21 +++++++++ src/components/RigidBodyComponents.cpp | 10 +++- src/components/RigidBodyComponents.h | 34 +++++++++++++- src/constraint/BallAndSocketJoint.cpp | 2 +- src/constraint/FixedJoint.cpp | 2 +- src/constraint/HingeJoint.cpp | 2 +- src/constraint/Joint.cpp | 3 +- src/constraint/Joint.h | 38 +--------------- src/constraint/SliderJoint.cpp | 2 +- src/engine/CollisionWorld.cpp | 17 +++---- src/engine/DynamicsWorld.cpp | 58 ++++++++---------------- src/engine/DynamicsWorld.h | 2 +- src/systems/ConstraintSolverSystem.cpp | 6 +-- src/systems/ContactSolverSystem.cpp | 2 +- 18 files changed, 115 insertions(+), 160 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index fbb8d262..74a1ccb7 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -40,16 +40,11 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(CollisionWorld& world, Entity entity) - : CollisionBody(world, entity), mMaterial(world.mConfig), mJointsList(nullptr), + : CollisionBody(world, entity), mMaterial(world.mConfig), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { } -// Destructor -RigidBody::~RigidBody() { - assert(mJointsList == nullptr); -} - // Return the type of the body BodyType RigidBody::getType() const { return mWorld.mRigidBodyComponents.getBodyType(mEntity); @@ -322,36 +317,6 @@ void RigidBody::setMass(decimal mass) { "Body " + std::to_string(mEntity.id) + ": Set mass=" + std::to_string(mass)); } -// Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) { - - assert(joint != nullptr); - assert(mJointsList != nullptr); - - // Remove the joint from the linked list of the joints of the first body - if (mJointsList->joint == joint) { // If the first element is the one to remove - JointListElement* elementToRemove = mJointsList; - mJointsList = elementToRemove->next; - elementToRemove->~JointListElement(); - memoryManager.release(MemoryManager::AllocationType::Pool, - elementToRemove, sizeof(JointListElement)); - } - else { // If the element to remove is not the first one in the list - JointListElement* currentElement = mJointsList; - while (currentElement->next != nullptr) { - if (currentElement->next->joint == joint) { - JointListElement* elementToRemove = currentElement->next; - currentElement->next = elementToRemove->next; - elementToRemove->~JointListElement(); - memoryManager.release(MemoryManager::AllocationType::Pool, - elementToRemove, sizeof(JointListElement)); - break; - } - currentElement = currentElement->next; - } - } -} - // Update the world inverse inertia tensor of the body /// The inertia tensor I_w in world coordinates is computed with the /// local inverse inertia tensor I_b^-1 in body coordinates diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index bd427657..3d1b5a6c 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -62,10 +62,6 @@ class RigidBody : public CollisionBody { /// Material properties of the rigid body 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 bool mIsCenterOfMassSetByUser; @@ -74,9 +70,6 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // - /// Remove a joint from the joints list - void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint); - /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); @@ -94,7 +87,7 @@ class RigidBody : public CollisionBody { RigidBody(CollisionWorld& world, Entity entity); /// Destructor - virtual ~RigidBody() override; + virtual ~RigidBody() override = default; /// Deleted copy-constructor RigidBody(const RigidBody& body) = delete; @@ -237,22 +230,6 @@ inline Material& RigidBody::getMaterial() { return mMaterial; } -// Return the first element of the linked list of joints involving this body -/** - * @return The first element of the linked-list of all the joints involving this body - */ -inline const JointListElement* RigidBody::getJointsList() const { - return mJointsList; -} - -// Return the first element of the linked list of joints involving this body -/** - * @return The first element of the linked-list of all the joints involving this body - */ -inline JointListElement* RigidBody::getJointsList() { - return mJointsList; -} - } #endif diff --git a/src/components/CollisionBodyComponents.h b/src/components/CollisionBodyComponents.h index 99b0a42d..8d1d3c39 100644 --- a/src/components/CollisionBodyComponents.h +++ b/src/components/CollisionBodyComponents.h @@ -107,7 +107,7 @@ class CollisionBodyComponents : public Components { /// Add a proxy-shape to a body component void addProxyShapeToBody(Entity bodyEntity, Entity proxyShapeEntity); - /// Set the transform of an entity + /// Remove a proxy-shape from a body component void removeProxyShapeFromBody(Entity bodyEntity, Entity proxyShapeEntity); /// Return a pointer to a body @@ -137,7 +137,7 @@ inline void CollisionBodyComponents::addProxyShapeToBody(Entity bodyEntity, Enti mProxyShapes[mMapEntityToComponentIndex[bodyEntity]].add(proxyShapeEntity); } -// Set the transform of an entity +// Remove a proxy-shape from a body component inline void CollisionBodyComponents::removeProxyShapeFromBody(Entity bodyEntity, Entity proxyShapeEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); diff --git a/src/components/JointComponents.cpp b/src/components/JointComponents.cpp index f867c3f1..71a33aba 100644 --- a/src/components/JointComponents.cpp +++ b/src/components/JointComponents.cpp @@ -34,7 +34,8 @@ using namespace reactphysics3d; // Constructor JointComponents::JointComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) + sizeof(Joint*) + - sizeof(JointType) + sizeof(JointsPositionCorrectionTechnique) + sizeof (bool)) { + sizeof(JointType) + sizeof(JointsPositionCorrectionTechnique) + sizeof(bool) + + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -60,6 +61,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { JointType* newTypes = reinterpret_cast(newJoints + nbComponentsToAllocate); JointsPositionCorrectionTechnique* newPositionCorrectionTechniques = reinterpret_cast(newTypes + nbComponentsToAllocate); bool* newIsCollisionEnabled = reinterpret_cast(newPositionCorrectionTechniques + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newIsCollisionEnabled + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -72,6 +74,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newTypes, mTypes, mNbComponents * sizeof(JointType)); memcpy(newPositionCorrectionTechniques, mPositionCorrectionTechniques, mNbComponents * sizeof(JointsPositionCorrectionTechnique)); memcpy(newIsCollisionEnabled, mIsCollisionEnabled, mNbComponents * sizeof(bool)); + memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -86,6 +89,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { mTypes = newTypes; mPositionCorrectionTechniques = newPositionCorrectionTechniques; mIsCollisionEnabled = newIsCollisionEnabled; + mIsAlreadyInIsland = newIsAlreadyInIsland; } // Add a component @@ -102,6 +106,7 @@ void JointComponents::addComponent(Entity jointEntity, bool isSleeping, const Jo new (mTypes + index) JointType(component.jointType); new (mPositionCorrectionTechniques + index) JointsPositionCorrectionTechnique(component.positionCorrectionTechnique); mIsCollisionEnabled[index] = component.isCollisionEnabled; + mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -126,6 +131,7 @@ void JointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { new (mTypes + destIndex) JointType(mTypes[srcIndex]); new (mPositionCorrectionTechniques + destIndex) JointsPositionCorrectionTechnique(mPositionCorrectionTechniques[srcIndex]); mIsCollisionEnabled[destIndex] = mIsCollisionEnabled[srcIndex]; + mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -149,6 +155,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { JointType jointType1(mTypes[index1]); JointsPositionCorrectionTechnique positionCorrectionTechnique1(mPositionCorrectionTechniques[index1]); bool isCollisionEnabled1 = mIsCollisionEnabled[index1]; + bool isAlreadyInIsland = mIsAlreadyInIsland[index1]; // Destroy component 1 destroyComponent(index1); @@ -163,6 +170,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { new (mTypes + index2) JointType(jointType1); new (mPositionCorrectionTechniques + index2) JointsPositionCorrectionTechnique(positionCorrectionTechnique1); mIsCollisionEnabled[index2] = isCollisionEnabled1; + mIsAlreadyInIsland[index2] = isAlreadyInIsland; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); diff --git a/src/components/JointComponents.h b/src/components/JointComponents.h index 7193620c..8f7d0eb2 100644 --- a/src/components/JointComponents.h +++ b/src/components/JointComponents.h @@ -73,6 +73,9 @@ class JointComponents : public Components { /// Array of boolean values to know if the two bodies of the constraint are allowed to collide with each other bool* mIsCollisionEnabled; + /// True if the joint has already been added into an island during islands creation + bool* mIsAlreadyInIsland; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -143,6 +146,12 @@ class JointComponents : public Components { /// Set whether the collision is enabled between the two bodies of a joint void setIsCollisionEnabled(Entity jointEntity, bool isCollisionEnabled); + /// Return true if the joint has already been added into an island during island creation + bool getIsAlreadyInIsland(Entity jointEntity) const; + + /// Set to true if the joint has already been added into an island during island creation + void setIsAlreadyInIsland(Entity jointEntity, bool isAlreadyInIsland); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -196,6 +205,18 @@ inline void JointComponents::setIsCollisionEnabled(Entity jointEntity, bool isCo mIsCollisionEnabled[mMapEntityToComponentIndex[jointEntity]] = isCollisionEnabled; } +// Return true if the joint has already been added into an island during island creation +inline bool JointComponents::getIsAlreadyInIsland(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsAlreadyInIsland[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set to true if the joint has already been added into an island during island creation +inline void JointComponents::setIsAlreadyInIsland(Entity jointEntity, bool isAlreadyInIsland) { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsAlreadyInIsland[mMapEntityToComponentIndex[jointEntity]] = isAlreadyInIsland; +} + } #endif diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index 7ad67fb9..a748f93e 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -43,7 +43,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(bool) + sizeof(bool)) { + sizeof(bool) + sizeof(bool) + sizeof(List)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -88,6 +88,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); + List* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -119,6 +120,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3)); memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); + memcpy(newJoints, mJoints, mNbComponents * sizeof(List)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -152,6 +154,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mCentersOfMassWorld = newCentersOfMassWorld; mIsGravityEnabled = newIsGravityEnabled; mIsAlreadyInIsland = newIsAlreadyInIsland; + mJoints = newJoints; } // Add a component @@ -187,6 +190,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mCentersOfMassWorld + index) Vector3(component.worldPosition); mIsGravityEnabled[index] = true; mIsAlreadyInIsland[index] = false; + new (mJoints + index) List(mMemoryAllocator); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -230,6 +234,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]); mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; + new (mJoints + destIndex) List(mJoints[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -272,6 +277,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1]; bool isGravityEnabled1 = mIsGravityEnabled[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; + List joints1 = mJoints[index1]; // Destroy component 1 destroyComponent(index1); @@ -305,6 +311,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mCentersOfMassWorld[index2] = centerOfMassWorld1; mIsGravityEnabled[index2] = isGravityEnabled1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; + new (mJoints + index2) List(joints1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -339,4 +346,5 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mConstrainedOrientations[index].~Quaternion(); mCentersOfMassLocal[index].~Vector3(); mCentersOfMassWorld[index].~Vector3(); + mJoints[index].~List(); } diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index 640a234b..23052176 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -142,6 +142,9 @@ class RigidBodyComponents : public Components { /// Array with the boolean value to know if the body has already been added into an island bool* mIsAlreadyInIsland; + /// For each body, the list of joints entities the body is part of + List* mJoints; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -330,6 +333,15 @@ class RigidBodyComponents : public Components { /// Set the value to know if the entity is already in an island void setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); + /// Return the list of joints of a body + const List& getJoints(Entity bodyEntity) const; + + /// Add a joint to a body component + void addJointToBody(Entity bodyEntity, Entity jointEntity); + + /// Remove a joint from a body component + void removeJointFromBody(Entity bodyEntity, Entity jointEntity); + // -------------------- Friendship -------------------- // friend class DynamicsWorld; @@ -729,10 +741,30 @@ inline void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isG inline void RigidBodyComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; } +// Return the list of joints of a body +inline const List& RigidBodyComponents::getJoints(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + return mJoints[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Add a joint to a body component +inline void RigidBodyComponents::addJointToBody(Entity bodyEntity, Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mJoints[mMapEntityToComponentIndex[bodyEntity]].add(jointEntity); +} + +// Remove a joint from a body component +inline void RigidBodyComponents::removeJointFromBody(Entity bodyEntity, Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mJoints[mMapEntityToComponentIndex[bodyEntity]].remove(jointEntity); +} + } #endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 16c6e5f4..f329cbdc 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -36,7 +36,7 @@ const decimal BallAndSocketJoint::BETA = decimal(0.2); // Constructor BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo) - : Joint(entity, world, jointInfo) { + : Joint(entity, world) { // Get the transforms of the two bodies Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 82b61b80..d62b26d5 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -36,7 +36,7 @@ const decimal FixedJoint::BETA = decimal(0.2); // Constructor FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo) - : Joint(entity, world, jointInfo) { + : Joint(entity, world) { // Compute the local-space anchor point for each body const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 113b68a4..546e1310 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; const decimal HingeJoint::BETA = decimal(0.2); // Constructor -HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world, jointInfo) { +HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world) { const decimal lowerLimit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity); const decimal upperLimit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity); diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp index 9bb68be3..d0826008 100644 --- a/src/constraint/Joint.cpp +++ b/src/constraint/Joint.cpp @@ -30,8 +30,7 @@ using namespace reactphysics3d; // Constructor -Joint::Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo) - :mEntity(entity), mWorld(world), mIsAlreadyInIsland(false) { +Joint::Joint(Entity entity, DynamicsWorld& world) :mEntity(entity), mWorld(world) { } diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 4d72c79f..20df2911 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -41,31 +41,6 @@ enum class JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; struct ConstraintSolverData; class Joint; -// Structure JointListElement -/** - * This structure represents a single element of a linked list of joints - */ -struct JointListElement { - - public: - - // -------------------- Attributes -------------------- // - - /// Pointer to the actual joint - Joint* joint; - - /// Next element of the list - JointListElement* next; - - // -------------------- Methods -------------------- // - - /// Constructor - JointListElement(Joint* initJoint, JointListElement* initNext) - :joint(initJoint), next(initNext){ - - } -}; - // Structure JointInfo /** * This structure is used to gather the information needed to create a joint. @@ -126,14 +101,8 @@ class Joint { /// Reference to the physics world DynamicsWorld& mWorld; - /// True if the joint has already been added into an island - bool mIsAlreadyInIsland; - // -------------------- Methods -------------------- // - /// Return true if the joint has already been added into an island - bool isAlreadyInIsland() const; - /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const = 0; @@ -157,7 +126,7 @@ class Joint { // -------------------- Methods -------------------- // /// Constructor - Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo); + Joint(Entity entity, DynamicsWorld& world); /// Destructor virtual ~Joint() = default; @@ -201,11 +170,6 @@ inline Entity Joint::getEntity() const { return mEntity; } -// Return true if the joint has already been added into an island -inline bool Joint::isAlreadyInIsland() const { - return mIsAlreadyInIsland; -} - } #endif diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index dafc5dd8..ab1fc812 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -36,7 +36,7 @@ const decimal SliderJoint::BETA = decimal(0.2); // Constructor SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo) - : Joint(entity, world, jointInfo) { + : Joint(entity, world) { assert(mWorld.mSliderJointsComponents.getUpperLimit(mEntity) >= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getLowerLimit(mEntity) <= decimal(0.0)); diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index b78eeb6a..c4a9351f 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -222,7 +222,6 @@ void CollisionWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) { if (mRigidBodyComponents.hasComponent(bodyEntity)) { mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); - mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled); } // For each proxy-shape of the body @@ -235,22 +234,24 @@ void CollisionWorld::setBodyDisabled(Entity bodyEntity, bool 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) { + const List& joints = mRigidBodyComponents.getJoints(bodyEntity); + for(uint32 i=0; i < joints.size(); i++) { + + Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); + Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); // If both bodies of the joint are disabled - if (mRigidBodyComponents.getIsEntityDisabled(jointElem->joint->getBody1()->getEntity()) && - mRigidBodyComponents.getIsEntityDisabled(jointElem->joint->getBody2()->getEntity())) { + if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && + mRigidBodyComponents.getIsEntityDisabled(body2Entity)) { // We disable the joint - setJointDisabled(jointElem->joint->getEntity(), true); + setJointDisabled(joints[i], true); } else { // Enable the joint - setJointDisabled(jointElem->joint->getEntity(), false); + setJointDisabled(joints[i], false); } } } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index db5a289c..ee07c254 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -289,8 +289,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Destroy all the joints in which the rigid body to be destroyed is involved JointListElement* element; - for (element = rigidBody->mJointsList; element != nullptr; element = element->next) { - destroyJoint(element->joint); + const List& joints = mRigidBodyComponents.getJoints(rigidBody->getEntity()); + for (uint32 i=0; i < joints.size(); i++) { + destroyJoint(mJointsComponents.getJoint(joints[i])); } // Destroy the corresponding entity and its components @@ -431,7 +432,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { "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); + addJointToBodies(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), entity); // Return the pointer to the created joint return newJoint; @@ -466,8 +467,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mJoints.remove(joint); // Remove the joint from the joint list of the bodies involved in the joint - body1->removeJointFromJointsList(mMemoryManager, joint); - body2->removeJointFromJointsList(mMemoryManager, joint); + mRigidBodyComponents.removeJointFromBody(body1->getEntity(), joint->getEntity()); + mRigidBodyComponents.removeJointFromBody(body2->getEntity(), joint->getEntity()); size_t nbBytes = joint->getSizeInBytes(); @@ -499,37 +500,17 @@ void DynamicsWorld::destroyJoint(Joint* joint) { } // Add the joint to the list of joints of the two bodies involved in the joint -void DynamicsWorld::addJointToBody(Joint* joint) { +void DynamicsWorld::addJointToBodies(Entity body1, Entity body2, Entity joint) { - assert(joint != nullptr); - - RigidBody* body1 = joint->getBody1(); - RigidBody* body2 = joint->getBody2(); - - // Add the joint at the beginning of the linked list of joints of the first body - void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(JointListElement)); - JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, - body1->mJointsList); - RigidBody* test1 = joint->getBody1(); - RigidBody* test2 = joint->getBody2(); - - body1->mJointsList = jointListElement1; + mRigidBodyComponents.addJointToBody(body1, joint); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(body1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + - " added to body"); + "Body " + std::to_string(body1.id) + ": Joint " + std::to_string(joint.id) + " added to body"); - // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(JointListElement)); - JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, - body2->mJointsList); - body2->mJointsList = jointListElement2; + mRigidBodyComponents.addJointToBody(body2, joint); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(body2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + - " added to body"); + "Body " + std::to_string(body2.id) + ": Joint " + std::to_string(joint.id) + " added to body"); } // Compute the islands using potential contacts and joints @@ -557,7 +538,7 @@ void DynamicsWorld::createIslands() { mRigidBodyComponents.mIsAlreadyInIsland[b] = false; } for (List::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { - (*it)->mIsAlreadyInIsland = false; + mJointsComponents.setIsAlreadyInIsland((*it)->getEntity(), false); } // Create a stack for the bodies to visit during the Depth First Search @@ -656,21 +637,20 @@ void DynamicsWorld::createIslands() { } // For each joint in which the current body is involved - JointListElement* jointElement; - for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr; - jointElement = jointElement->next) { + const List& joints = mRigidBodyComponents.getJoints(rigidBodyToVisit->getEntity()); + for (uint32 i=0; i < joints.size(); i++) { - Joint* joint = jointElement->joint; + Joint* joint = mJointsComponents.getJoint(joints[i]); // Check if the current joint has already been added into an island - if (joint->isAlreadyInIsland()) continue; + if (mJointsComponents.getIsAlreadyInIsland(joints[i])) continue; // Add the joint into the island mIslands.joints[islandIndex].add(joint); - joint->mIsAlreadyInIsland = true; + mJointsComponents.setIsAlreadyInIsland(joints[i], true); - const Entity body1Entity = joint->getBody1()->getEntity(); - const Entity body2Entity = joint->getBody2()->getEntity(); + const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]); + const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]); const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity; // Check if the other body has already been added to the island diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 5687c943..a8f32617 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -120,7 +120,7 @@ class DynamicsWorld : public CollisionWorld { void updateSleepingBodies(decimal timeStep); /// Add the joint to the list of joints of the two bodies involved in the joint - void addJointToBody(Joint* joint); + void addJointToBodies(Entity body1, Entity body2, Entity joint); public : diff --git a/src/systems/ConstraintSolverSystem.cpp b/src/systems/ConstraintSolverSystem.cpp index ae637ba7..4a8d49eb 100644 --- a/src/systems/ConstraintSolverSystem.cpp +++ b/src/systems/ConstraintSolverSystem.cpp @@ -46,7 +46,7 @@ ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyCompon // Initialize the constraint solver for a given island void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) { - RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler); + RP3D_PROFILE("ConstraintSolverSystem::initializeForIsland()", mProfiler); assert(mIslands.bodyEntities[islandIndex].size() > 0); assert(mIslands.joints[islandIndex].size() > 0); @@ -74,7 +74,7 @@ void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) { // Solve the velocity constraints void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) { - RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler); + RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler); assert(mIslands.joints[islandIndex].size() > 0); @@ -89,7 +89,7 @@ void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) { // Solve the position constraints void ConstraintSolverSystem::solvePositionConstraints(uint islandIndex) { - RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler); + RP3D_PROFILE("ConstraintSolverSystem::solvePositionConstraints()", mProfiler); assert(mIslands.joints[islandIndex].size() > 0); diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 03e1a95c..3ffbc384 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -488,7 +488,7 @@ void ContactSolverSystem::warmStart() { // Solve the contacts void ContactSolverSystem::solve() { - RP3D_PROFILE("ContactSolver::solve()", mProfiler); + RP3D_PROFILE("ContactSolverSystem::solve()", mProfiler); decimal deltaLambda; decimal lambdaTemp;