mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on JointComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-09-24 17:45:43 +02:00
parent 170a1bfdfd
commit f29810334e
18 changed files with 115 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/body/RigidBody.cpp
View File

@ -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

25
src/body/RigidBody.h
View File

@ -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

4
src/components/CollisionBodyComponents.h
View File

@ -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));

10
src/components/JointComponents.cpp
View File

@ -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<JointType*>(newJoints + nbComponentsToAllocate);
JointsPositionCorrectionTechnique* newPositionCorrectionTechniques = reinterpret_cast<JointsPositionCorrectionTechnique*>(newTypes + nbComponentsToAllocate);
bool* newIsCollisionEnabled = reinterpret_cast<bool*>(newPositionCorrectionTechniques + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(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<Entity, uint32>(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<Entity, uint32>(jointEntity1, index2));

21
src/components/JointComponents.h
View File

@ -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

10
src/components/RigidBodyComponents.cpp
View File

@ -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<Entity>)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -88,6 +88,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
List<Entity>* newJoints = reinterpret_cast<List<Entity>*>(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<Entity>));
// 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<Entity>(mMemoryAllocator);
// Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(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<Entity>(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<Entity> 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<Entity>(joints1);
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
@ -339,4 +346,5 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
mConstrainedOrientations[index].~Quaternion();
mCentersOfMassLocal[index].~Vector3();
mCentersOfMassWorld[index].~Vector3();
mJoints[index].~List<Entity>();
}

34
src/components/RigidBodyComponents.h
View File

@ -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<Entity>* 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<Entity>& 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<Entity>& 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

2
src/constraint/BallAndSocketJoint.cpp
View File

@ -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());

2
src/constraint/FixedJoint.cpp
View File

@ -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());

2
src/constraint/HingeJoint.cpp
View File

@ -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);

3
src/constraint/Joint.cpp
View File

@ -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) {
}

38
src/constraint/Joint.h
View File

@ -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

2
src/constraint/SliderJoint.cpp
View File

@ -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));

17
src/engine/CollisionWorld.cpp
View File

@ -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<Entity>& 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);
}
}
}

58
src/engine/DynamicsWorld.cpp
View File

@ -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<Entity>& 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<Joint*>::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<Entity>& 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

2
src/engine/DynamicsWorld.h
View File

@ -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 :

6
src/systems/ConstraintSolverSystem.cpp
View File

@ -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);

2
src/systems/ContactSolverSystem.cpp
View File

@ -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;