From 8187c19fa3c15b7b8b865ad4c0ed9a4f6652bf75 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 2 Sep 2019 14:15:03 +0200 Subject: [PATCH] Add BallAndSocketJointComponents class --- CMakeLists.txt | 2 + src/collision/shapes/CollisionShape.h | 3 - src/collision/shapes/ConcaveMeshShape.h | 2 +- src/collision/shapes/SphereShape.h | 2 +- .../BallAndSocketJointComponents.cpp | 226 ++++++++++++ src/components/BallAndSocketJointComponents.h | 331 ++++++++++++++++++ src/constraint/BallAndSocketJoint.cpp | 135 ++++--- src/constraint/BallAndSocketJoint.h | 32 -- src/constraint/Joint.h | 3 - src/engine/CollisionWorld.cpp | 2 +- src/engine/CollisionWorld.h | 4 + src/engine/DynamicsWorld.cpp | 17 +- src/engine/DynamicsWorld.h | 1 + 13 files changed, 666 insertions(+), 94 deletions(-) create mode 100644 src/components/BallAndSocketJointComponents.cpp create mode 100644 src/components/BallAndSocketJointComponents.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 640cd580..138b5e9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,6 +147,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/TransformComponents.h" "src/components/ProxyShapeComponents.h" "src/components/JointComponents.h" + "src/components/BallAndSocketJointComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -240,6 +241,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/TransformComponents.cpp" "src/components/ProxyShapeComponents.cpp" "src/components/JointComponents.cpp" + "src/components/BallAndSocketJointComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" "src/mathematics/mathematics_functions.cpp" diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 4e1206d3..2f2d5611 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -123,9 +123,6 @@ class CollisionShape { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; - /// Return the scaling vector of the collision shape - Vector3 getLocalScaling() const; - /// Return the id of the shape uint getId() const; diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 27276bcb..a45d607a 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -175,7 +175,7 @@ class ConcaveMeshShape : public ConcaveShape { ConcaveMeshShape(TriangleMesh* triangleMesh, const Vector3& scaling = Vector3(1, 1, 1)); /// Destructor - virtual ~ConcaveMeshShape() = default; + virtual ~ConcaveMeshShape() override = default; /// Deleted copy-constructor ConcaveMeshShape(const ConcaveMeshShape& shape) = delete; diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index d7c75bba..a238d7a3 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -110,7 +110,7 @@ inline decimal SphereShape::getRadius() const { * @return False because the sphere shape is not a polyhedron */ inline bool SphereShape::isPolyhedron() const { - return false; + return false; } // Return the number of bytes used by the collision shape diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp new file mode 100644 index 00000000..f1791575 --- /dev/null +++ b/src/components/BallAndSocketJointComponents.cpp @@ -0,0 +1,226 @@ +/******************************************************************************** +* 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 "BallAndSocketJointComponents.h" +#include "engine/EntityManager.h" +#include "mathematics/Matrix3x3.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(BallAndSocketJoint*) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Vector3)) { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + +// Allocate memory for a given number of components +void BallAndSocketJointComponents::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* newJointEntities = static_cast(newBuffer); + BallAndSocketJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); + Vector3* newR1World = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); + Vector3* newR2World = reinterpret_cast(newR1World + nbComponentsToAllocate); + Matrix3x3* newI1 = reinterpret_cast(newR2World + nbComponentsToAllocate); + Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); + Vector3* newBiasVector = reinterpret_cast(newI2 + nbComponentsToAllocate); + Matrix3x3* newInverseMassMatrix = reinterpret_cast(newBiasVector + nbComponentsToAllocate); + Vector3* newImpulse = reinterpret_cast(newInverseMassMatrix + nbComponentsToAllocate); + + // If there was already components before + if (mNbComponents > 0) { + + // Copy component data from the previous buffer to the new one + memcpy(newJointEntities, mJointEntities, mNbComponents * sizeof(Entity)); + memcpy(newJoints, mJoints, mNbComponents * sizeof(BallAndSocketJoint*)); + memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3)); + memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3)); + memcpy(newR1World, mR1World, mNbComponents * sizeof(Vector3)); + memcpy(newR2World, mR2World, mNbComponents * sizeof(Vector3)); + memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3)); + memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3)); + memcpy(newBiasVector, mBiasVector, mNbComponents * sizeof(Vector3)); + memcpy(newInverseMassMatrix, mInverseMassMatrix, mNbComponents * sizeof(Matrix3x3)); + memcpy(newImpulse, mImpulse, mNbComponents * sizeof(Vector3)); + + // Deallocate previous memory + mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); + } + + mBuffer = newBuffer; + mJointEntities = newJointEntities; + mJoints = newJoints; + mNbAllocatedComponents = nbComponentsToAllocate; + mLocalAnchorPointBody1 = newLocalAnchorPointBody1; + mLocalAnchorPointBody2 = newLocalAnchorPointBody2; + mR1World = newR1World; + mR2World = newR2World; + mI1 = newI1; + mI2 = newI2; + mBiasVector = newBiasVector; + mInverseMassMatrix = newInverseMassMatrix; + mImpulse = newImpulse; +} + +// Add a component +void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleeping, const BallAndSocketJointComponent& 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); + mJoints[index] = nullptr; + new (mLocalAnchorPointBody1 + index) Vector3(0, 0, 0); + new (mLocalAnchorPointBody2 + index) Vector3(0, 0, 0); + new (mR1World + index) Vector3(0, 0, 0); + new (mR2World + index) Vector3(0, 0, 0); + new (mI1 + index) Matrix3x3(); + new (mI2 + index) Matrix3x3(); + new (mBiasVector + index) Vector3(0, 0, 0); + new (mInverseMassMatrix + index) Matrix3x3(); + new (mImpulse + index) Vector3(0, 0, 0); + + // 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 BallAndSocketJointComponents::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]); + mJoints[destIndex] = mJoints[srcIndex]; + new (mLocalAnchorPointBody1 + destIndex) Vector3(mLocalAnchorPointBody1[srcIndex]); + new (mLocalAnchorPointBody2 + destIndex) Vector3(mLocalAnchorPointBody2[srcIndex]); + new (mR1World + destIndex) Vector3(mR1World[srcIndex]); + new (mR2World + destIndex) Vector3(mR2World[srcIndex]); + new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]); + new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]); + new (mBiasVector + destIndex) Vector3(mBiasVector[srcIndex]); + new (mInverseMassMatrix + destIndex) Matrix3x3(mInverseMassMatrix[srcIndex]); + new (mImpulse + destIndex) Vector3(mImpulse[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 BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity jointEntity1(mJointEntities[index1]); + BallAndSocketJoint* joint1 = mJoints[index1]; + Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]); + Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]); + Vector3 r1World1(mR1World[index1]); + Vector3 r2World1(mR2World[index1]); + Matrix3x3 i11(mI1[index1]); + Matrix3x3 i21(mI2[index1]); + Vector3 biasVector1(mBiasVector[index1]); + Matrix3x3 inverseMassMatrix1(mInverseMassMatrix[index1]); + Vector3 impulse1(mImpulse[index1]); + + // Destroy component 1 + destroyComponent(index1); + + moveComponentToIndex(index2, index1); + + // Reconstruct component 1 at component 2 location + new (mJointEntities + index2) Entity(jointEntity1); + mJoints[index2] = joint1; + new (mLocalAnchorPointBody1 + index2) Vector3(localAnchorPointBody1); + new (mLocalAnchorPointBody2 + index2) Vector3(localAnchorPointBody2); + new (mR1World + index2) Vector3(r1World1); + new (mR2World + index2) Vector3(r2World1); + new (mI1 + index2) Matrix3x3(i11); + new (mI2 + index2) Matrix3x3(i21); + new (mBiasVector + index2) Vector3(biasVector1); + new (mInverseMassMatrix + index2) Matrix3x3(inverseMassMatrix1); + new (mImpulse + index2) Vector3(impulse1); + + // 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 BallAndSocketJointComponents::destroyComponent(uint32 index) { + + Components::destroyComponent(index); + + assert(mMapEntityToComponentIndex[mJointEntities[index]] == index); + + mMapEntityToComponentIndex.remove(mJointEntities[index]); + + mJointEntities[index].~Entity(); + mJoints[index] = nullptr; + mLocalAnchorPointBody1[index].~Vector3(); + mLocalAnchorPointBody2[index].~Vector3(); + mR1World[index].~Vector3(); + mR2World[index].~Vector3(); + mI1[index].~Matrix3x3(); + mI2[index].~Matrix3x3(); + mBiasVector[index].~Vector3(); + mInverseMassMatrix[index].~Matrix3x3(); + mImpulse[index].~Vector3(); +} diff --git a/src/components/BallAndSocketJointComponents.h b/src/components/BallAndSocketJointComponents.h new file mode 100644 index 00000000..13d58eab --- /dev/null +++ b/src/components/BallAndSocketJointComponents.h @@ -0,0 +1,331 @@ +/******************************************************************************** +* 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_BALL_AND_SOCKET_JOINT_COMPONENTS_H +#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_COMPONENTS_H + +// Libraries +#include "mathematics/Transform.h" +#include "mathematics/Matrix3x3.h" +#include "engine/Entity.h" +#include "components/Components.h" +#include "containers/Map.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class declarations +class MemoryAllocator; +class EntityManager; +class BallAndSocketJoint; +enum class JointType; + +// Class BallAndSocketJointComponents +/** + * This class represent the component of the ECS with data for the BallAndSocketJoint. + */ +class BallAndSocketJointComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of joint entities + Entity* mJointEntities; + + /// Array of pointers to the joints + BallAndSocketJoint** mJoints; + + /// Anchor point of body 1 (in local-space coordinates of body 1) + Vector3* mLocalAnchorPointBody1; + + /// Anchor point of body 2 (in local-space coordinates of body 2) + Vector3* mLocalAnchorPointBody2; + + /// Vector from center of body 2 to anchor point in world-space + Vector3* mR1World; + + /// Vector from center of body 2 to anchor point in world-space + Vector3* mR2World; + + /// Inertia tensor of body 1 (in world-space coordinates) + Matrix3x3* mI1; + + /// Inertia tensor of body 2 (in world-space coordinates) + Matrix3x3* mI2; + + /// Bias vector for the constraint + Vector3* mBiasVector; + + /// Inverse mass matrix K=JM^-1J^-t of the constraint + Matrix3x3* mInverseMassMatrix; + + /// Accumulated impulse + Vector3* mImpulse; + + // -------------------- 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 BallAndSocketJointComponent { + + /// Constructor + BallAndSocketJointComponent() { + + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + BallAndSocketJointComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~BallAndSocketJointComponents() override = default; + + /// Add a component + void addComponent(Entity jointEntity, bool isSleeping, const BallAndSocketJointComponent& component); + + /// Return a pointer to a given joint + BallAndSocketJoint* getJoint(Entity jointEntity) const; + + /// Set the joint pointer to a given joint + void setJoint(Entity jointEntity, BallAndSocketJoint* joint) const; + + /// Return the local anchor point of body 1 for a given joint + const Vector3& getLocalAnchoirPointBody1(Entity jointEntity) const; + + /// Set the local anchor point of body 1 for a given joint + void setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1); + + /// Return the local anchor point of body 2 for a given joint + const Vector3& getLocalAnchoirPointBody2(Entity jointEntity) const; + + /// Set the local anchor point of body 2 for a given joint + void setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2); + + /// Return the vector from center of body 1 to anchor point in world-space + const Vector3& getR1World(Entity jointEntity) const; + + /// Set the vector from center of body 1 to anchor point in world-space + void setR1World(Entity jointEntity, const Vector3& r1World); + + /// Return the vector from center of body 2 to anchor point in world-space + const Vector3& getR2World(Entity jointEntity) const; + + /// Set the vector from center of body 2 to anchor point in world-space + void setR2World(Entity jointEntity, const Vector3& r2World); + + /// Return the inertia tensor of body 1 (in world-space coordinates) + const Matrix3x3& getI1(Entity jointEntity) const; + + /// Set the inertia tensor of body 1 (in world-space coordinates) + void setI1(Entity jointEntity, const Matrix3x3& i1); + + /// Return the inertia tensor of body 2 (in world-space coordinates) + const Matrix3x3& getI2(Entity jointEntity) const; + + /// Set the inertia tensor of body 2 (in world-space coordinates) + void setI2(Entity jointEntity, const Matrix3x3& i2); + + /// Return the bias vector for the constraint + Vector3& getBiasVector(Entity jointEntity); + + /// Set the bias vector for the constraint + void setBiasVector(Entity jointEntity, const Vector3& biasVector); + + /// Return the inverse mass matrix K=JM^-1J^-t of the constraint + Matrix3x3& getInverseMassMatrix(Entity jointEntity); + + /// Set the inverse mass matrix K=JM^-1J^-t of the constraint + void setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix); + + /// Return the accumulated impulse + Vector3& getImpulse(Entity jointEntity); + + /// Set the accumulated impulse + void setImpulse(Entity jointEntity, const Vector3& impulse); + + // -------------------- Friendship -------------------- // + + friend class BroadPhaseSystem; +}; + +// Return a pointer to a given joint +inline BallAndSocketJoint* BallAndSocketJointComponents::getJoint(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mJoints[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the joint pointer to a given joint +inline void BallAndSocketJointComponents::setJoint(Entity jointEntity, BallAndSocketJoint* joint) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mJoints[mMapEntityToComponentIndex[jointEntity]] = joint; +} + +// Return the local anchor point of body 1 for a given joint +inline const Vector3& BallAndSocketJointComponents::getLocalAnchoirPointBody1(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the local anchor point of body 1 for a given joint +inline void BallAndSocketJointComponents::setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1; +} + +// Return the local anchor point of body 2 for a given joint +inline const Vector3& BallAndSocketJointComponents::getLocalAnchoirPointBody2(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the local anchor point of body 2 for a given joint +inline void BallAndSocketJointComponents::setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; +} + +// Return the vector from center of body 1 to anchor point in world-space +inline const Vector3& BallAndSocketJointComponents::getR1World(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mR1World[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the vector from center of body 1 to anchor point in world-space +inline void BallAndSocketJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World; +} + +// Return the vector from center of body 2 to anchor point in world-space +inline const Vector3& BallAndSocketJointComponents::getR2World(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mR2World[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the vector from center of body 2 to anchor point in world-space +inline void BallAndSocketJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World; +} + +// Return the inertia tensor of body 1 (in world-space coordinates) +inline const Matrix3x3& BallAndSocketJointComponents::getI1(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mI1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the inertia tensor of body 1 (in world-space coordinates) +inline void BallAndSocketJointComponents::setI1(Entity jointEntity, const Matrix3x3& i1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mI1[mMapEntityToComponentIndex[jointEntity]] = i1; +} + +// Return the inertia tensor of body 2 (in world-space coordinates) +inline const Matrix3x3& BallAndSocketJointComponents::getI2(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mI2[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the inertia tensor of body 2 (in world-space coordinates) +inline void BallAndSocketJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mI2[mMapEntityToComponentIndex[jointEntity]] = i2; +} + +// Return the bias vector for the constraint +inline Vector3 &BallAndSocketJointComponents::getBiasVector(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasVector[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the bias vector for the constraint +inline void BallAndSocketJointComponents::setBiasVector(Entity jointEntity, const Vector3& biasVector) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasVector[mMapEntityToComponentIndex[jointEntity]] = biasVector; +} + +// Return the inverse mass matrix K=JM^-1J^-t of the constraint +inline Matrix3x3& BallAndSocketJointComponents::getInverseMassMatrix(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the inverse mass matrix K=JM^-1J^-t of the constraint +inline void BallAndSocketJointComponents::setInverseMassMatrix(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrix[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the accumulated impulse +inline Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulse[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the accumulated impulse +inline void BallAndSocketJointComponents::setImpulse(Entity jointEntity, const Vector3& impulse) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulse[mMapEntityToComponentIndex[jointEntity]] = impulse; +} + +} + +#endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 8bd52a0d..16c6e5f4 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -36,15 +36,15 @@ const decimal BallAndSocketJoint::BETA = decimal(0.2); // Constructor BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo) - : Joint(entity, world, jointInfo), mImpulse(Vector3(0, 0, 0)) { + : Joint(entity, world, jointInfo) { // Get the transforms of the two bodies Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = body1Transform.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = body2Transform.getInverse() * jointInfo.anchorPointWorldSpace; + mWorld.mBallAndSocketJointsComponents.setLocalAnchoirPointBody1(entity, body1Transform.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mBallAndSocketJointsComponents.setLocalAnchoirPointBody2(entity, body2Transform.getInverse() * jointInfo.anchorPointWorldSpace); } // Initialize before solving the constraint @@ -65,46 +65,55 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS const Quaternion& orientationBody2 = body2->getTransform().getOrientation(); // Get the inertia tensor of bodies - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mBallAndSocketJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mBallAndSocketJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + const Vector3 localAnchorPointBody1 = mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity); + const Vector3 localAnchorPointBody2 = mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity); + mWorld.mBallAndSocketJointsComponents.setR1World(mEntity, orientationBody1 * localAnchorPointBody1); + mWorld.mBallAndSocketJointsComponents.setR2World(mEntity, orientationBody2 * localAnchorPointBody2); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity); + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); - decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); - decimal inverseMassBodies = body1MassInverse + body2MassInverse; + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); + const decimal inverseMassBodies = body1MassInverse + body2MassInverse; + const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity); Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); + skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose(); // Compute the inverse mass matrix K^-1 - mInverseMassMatrix.setToZero(); + Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity); + inverseMassMatrix.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrix = massMatrix.getInverse(); + mWorld.mBallAndSocketJointsComponents.setInverseMassMatrix(mEntity, massMatrix.getInverse()); } // Compute the bias "b" of the constraint - mBiasVector.setToZero(); + Vector3& biasVector = mWorld.mBallAndSocketJointsComponents.getBiasVector(mEntity); + biasVector.setToZero(); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { decimal biasFactor = (BETA / constraintSolverData.timeStep); - mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World); + mWorld.mBallAndSocketJointsComponents.setBiasVector(mEntity, biasFactor * (x2 + r2World - x1 - r1World)); } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { // Reset the accumulated impulse - mImpulse.setToZero(); + Vector3& impulse = mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity); + impulse.setToZero(); } } @@ -123,20 +132,27 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity); + // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -mImpulse; - const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); + const Vector3& impulse = mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity); + const Vector3 linearImpulseBody1 = -impulse; + const Vector3 angularImpulseBody1 = impulse.cross(r1World); // Apply the impulse to the body 1 v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); + const Vector3 angularImpulseBody2 = -impulse.cross(r2World); // Apply the impulse to the body to the body 2 - v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * mImpulse; - w2 += mI2 * angularImpulseBody2; + v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * impulse; + w2 += i2 * angularImpulseBody2; } // Solve the velocity constraint @@ -154,27 +170,36 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity); + + const Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity); + const Vector3& biasVector = mWorld.mBallAndSocketJointsComponents.getBiasVector(mEntity); + // Compute J*v - const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + const Vector3 Jv = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector); - mImpulse += deltaLambda; + const Vector3 deltaLambda = inverseMassMatrix * (-Jv - biasVector); + mWorld.mBallAndSocketJointsComponents.setImpulse(mEntity, mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity) + deltaLambda); // Compute the impulse P=J^T * lambda for the body 1 const Vector3 linearImpulseBody1 = -deltaLambda; - const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + const Vector3 angularImpulseBody1 = deltaLambda.cross(r1World); // Apply the impulse to the body 1 v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); // Apply the impulse to the body 2 v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * deltaLambda; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } // Solve the position constraint (for position error correction) @@ -201,46 +226,53 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& r1World = mWorld.mBallAndSocketJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mBallAndSocketJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mBallAndSocketJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mBallAndSocketJointsComponents.getI2(mEntity); + // Recompute the inverse inertia tensors - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mBallAndSocketJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mBallAndSocketJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + mWorld.mBallAndSocketJointsComponents.setR1World(mEntity, q1 * mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity)); + mWorld.mBallAndSocketJointsComponents.setR2World(mEntity, q2 * mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity)); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation constraints decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrix.setToZero(); + skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose(); + Matrix3x3& inverseMassMatrix = mWorld.mBallAndSocketJointsComponents.getInverseMassMatrix(mEntity); + inverseMassMatrix.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrix = massMatrix.getInverse(); + mWorld.mBallAndSocketJointsComponents.setInverseMassMatrix(mEntity, massMatrix.getInverse()); } // Compute the constraint error (value of the C(x) function) - const Vector3 constraintError = (x2 + mR2World - x1 - mR1World); + const Vector3 constraintError = (x2 + r2World - x1 - r1World); // Compute the Lagrange multiplier lambda // TODO : Do not solve the system by computing the inverse each time and multiplying with the // right-hand side vector but instead use a method to directly solve the linear system. - const Vector3 lambda = mInverseMassMatrix * (-constraintError); + const Vector3 lambda = inverseMassMatrix * (-constraintError); // Compute the impulse of body 1 const Vector3 linearImpulseBody1 = -lambda; - const Vector3 angularImpulseBody1 = lambda.cross(mR1World); + const Vector3 angularImpulseBody1 = lambda.cross(r1World); // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + const Vector3 w1 = i1 * angularImpulseBody1; // Update the body center of mass and orientation of body 1 x1 += v1; @@ -248,11 +280,11 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con q1.normalize(); // Compute the impulse of body 2 - const Vector3 angularImpulseBody2 = -lambda.cross(mR2World); + const Vector3 angularImpulseBody2 = -lambda.cross(r2World); // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * lambda; - const Vector3 w2 = mI2 * angularImpulseBody2; + const Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -265,3 +297,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } +// Return a string representation +std::string BallAndSocketJoint::to_string() const { + + return "BallAndSocketJoint{ localAnchorPointBody1=" + mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() + + ", localAnchorPointBody2=" + mWorld.mBallAndSocketJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() + "}"; +} + diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index f61e336b..533b57c2 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -76,32 +76,6 @@ class BallAndSocketJoint : public Joint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; - - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; - - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR1World; - - /// Vector from center of body 2 to anchor point in world-space - Vector3 mR2World; - - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; - - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; - - /// Bias vector for the constraint - Vector3 mBiasVector; - - /// Inverse mass matrix K=JM^-1J^-t of the constraint - Matrix3x3 mInverseMassMatrix; - - /// Accumulated impulse - Vector3 mImpulse; // -------------------- Methods -------------------- // @@ -145,12 +119,6 @@ inline size_t BallAndSocketJoint::getSizeInBytes() const { return sizeof(BallAndSocketJoint); } -// Return a string representation -inline std::string BallAndSocketJoint::to_string() const { - return "BallAndSocketJoint{ localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() + - ", localAnchorPointBody2=" + mLocalAnchorPointBody2.to_string() + "}"; -} - } #endif diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index eabd11dc..4d72c79f 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -129,9 +129,6 @@ class Joint { /// True if the joint has already been added into an island bool mIsAlreadyInIsland; - /// Total number of joints - static uint mNbTotalNbJoints; - // -------------------- Methods -------------------- // /// Return true if the joint has already been added into an island diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 33b11b48..39fc3531 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -40,7 +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()), + mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index c74e8580..007c3a9e 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -38,6 +38,7 @@ #include "components/TransformComponents.h" #include "components/ProxyShapeComponents.h" #include "components/JointComponents.h" +#include "components/BallAndSocketJointComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -92,6 +93,9 @@ class CollisionWorld { /// Joint Components JointComponents mJointsComponents; + /// Ball And Socket joints Components + BallAndSocketJointComponents mBallAndSocketJointsComponents; + /// Reference to the collision detection CollisionDetectionSystem mCollisionDetection; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 65041ed8..1a67ca18 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -322,17 +322,26 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { Joint* newJoint = nullptr; + const bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && + mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); + // Allocate memory to create the new joint switch(jointInfo.type) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { + // Create a BallAndSocketJoint component + BallAndSocketJointComponents::BallAndSocketJointComponent ballAndSocketJointComponent; + mBallAndSocketJointsComponents.addComponent(entity, isJointDisabled, ballAndSocketJointComponent); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(BallAndSocketJoint)); - const BallAndSocketJointInfo& info = static_cast( - jointInfo); - newJoint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info); + const BallAndSocketJointInfo& info = static_cast(jointInfo); + BallAndSocketJoint* joint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info); + + newJoint = joint; + mBallAndSocketJointsComponents.setJoint(entity, joint); break; } @@ -373,8 +382,6 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { } } - bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && - mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint, jointInfo.type, jointInfo.positionCorrectionTechnique, jointInfo.isCollisionEnabled); mJointsComponents.addComponent(entity, isJointDisabled, jointComponent); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 95296bd5..839e5ffe 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -80,6 +80,7 @@ class DynamicsWorld : public CollisionWorld { List mRigidBodies; /// All the joints of the world + // TODO : We probably do not need this list anymore List mJoints; /// Gravity vector of the world