diff --git a/CMakeLists.txt b/CMakeLists.txt index 138b5e9f..0d90813e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,6 +148,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/ProxyShapeComponents.h" "src/components/JointComponents.h" "src/components/BallAndSocketJointComponents.h" + "src/components/FixedJointComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -242,6 +243,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/ProxyShapeComponents.cpp" "src/components/JointComponents.cpp" "src/components/BallAndSocketJointComponents.cpp" + "src/components/FixedJointComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" "src/mathematics/mathematics_functions.cpp" diff --git a/src/components/FixedJointComponents.cpp b/src/components/FixedJointComponents.cpp new file mode 100644 index 00000000..fb261de0 --- /dev/null +++ b/src/components/FixedJointComponents.cpp @@ -0,0 +1,259 @@ +/******************************************************************************** +* 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 "FixedJointComponents.h" +#include "engine/EntityManager.h" +#include "mathematics/Matrix3x3.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +FixedJointComponents::FixedJointComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(FixedJoint*) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion)) { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + +// Allocate memory for a given number of components +void FixedJointComponents::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); + FixedJoint** 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* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); + Vector3* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); + Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); + Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); + Vector3* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); + Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); + Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + 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(FixedJoint*)); + 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(newImpulseTranslation, mImpulseTranslation, mNbComponents * sizeof(Vector3)); + memcpy(newImpulseRotation, mImpulseRotation, mNbComponents * sizeof(Vector3)); + memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix3x3)); + memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix3x3)); + memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector3)); + memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3)); + memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion)); + + // 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; + mImpulseTranslation = newImpulseTranslation; + mImpulseRotation = newImpulseRotation; + mInverseMassMatrixTranslation = newInverseMassMatrixTranslation; + mInverseMassMatrixRotation = newInverseMassMatrixRotation; + mBiasTranslation = newBiasTranslation; + mBiasRotation = newBiasRotation; + mInitOrientationDifferenceInv = newInitOrientationDifferenceInv; +} + +// Add a component +void FixedJointComponents::addComponent(Entity jointEntity, bool isSleeping, const FixedJointComponent& 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 (mImpulseTranslation + index) Vector3(0, 0, 0); + new (mImpulseRotation + index) Vector3(0, 0, 0); + new (mInverseMassMatrixTranslation + index) Matrix3x3(); + new (mInverseMassMatrixRotation + index) Matrix3x3(); + new (mBiasTranslation + index) Vector3(0, 0, 0); + new (mBiasRotation + index) Vector3(0, 0, 0); + new (mInitOrientationDifferenceInv + index) Quaternion(0, 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 FixedJointComponents::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 (mImpulseTranslation + destIndex) Vector3(mImpulseRotation[srcIndex]); + new (mImpulseRotation + destIndex) Vector3(mImpulseRotation[srcIndex]); + new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]); + new (mInverseMassMatrixRotation + destIndex) Matrix3x3(mInverseMassMatrixRotation[srcIndex]); + new (mBiasTranslation + destIndex) Vector3(mBiasTranslation[srcIndex]); + new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]); + new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[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 FixedJointComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity jointEntity1(mJointEntities[index1]); + FixedJoint* 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 impulseTranslation1(mImpulseTranslation[index1]); + Vector3 impulseRotation1(mImpulseRotation[index1]); + Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]); + Matrix3x3 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]); + Vector3 biasTranslation1(mBiasTranslation[index1]); + Vector3 biasRotation1(mBiasRotation[index1]); + Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[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 (mImpulseTranslation + index2) Vector3(impulseTranslation1); + new (mImpulseRotation + index2) Vector3(impulseRotation1); + new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1); + new (mInverseMassMatrixRotation + index2) Matrix3x3(inverseMassMatrixRotation1); + new (mBiasTranslation + index2) Vector3(biasTranslation1); + new (mBiasRotation + index2) Vector3(biasRotation1); + new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); + + // 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 FixedJointComponents::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(); + mImpulseTranslation[index].~Vector3(); + mImpulseRotation[index].~Vector3(); + mInverseMassMatrixTranslation[index].~Matrix3x3(); + mInverseMassMatrixRotation[index].~Matrix3x3(); + mBiasTranslation[index].~Vector3(); + mBiasRotation[index].~Vector3(); + mInitOrientationDifferenceInv[index].~Quaternion(); +} diff --git a/src/components/FixedJointComponents.h b/src/components/FixedJointComponents.h new file mode 100644 index 00000000..bfaed648 --- /dev/null +++ b/src/components/FixedJointComponents.h @@ -0,0 +1,424 @@ +/******************************************************************************** +* 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_FIXED_JOINT_COMPONENTS_H +#define REACTPHYSICS3D_FIXED_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 FixedJoint; +enum class JointType; + +// Class FixedJointComponents +/** + * This class represent the component of the ECS with data for the FixedJoint. + */ +class FixedJointComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of joint entities + Entity* mJointEntities; + + /// Array of pointers to the joints + FixedJoint** 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; + + /// Accumulated impulse for the 3 translation constraints + Vector3* mImpulseTranslation; + + /// Accumulate impulse for the 3 rotation constraints + Vector3* mImpulseRotation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) + Matrix3x3* mInverseMassMatrixTranslation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) + Matrix3x3* mInverseMassMatrixRotation; + + /// Bias vector for the 3 translation constraints + Vector3* mBiasTranslation; + + /// Bias vector for the 3 rotation constraints + Vector3* mBiasRotation; + + /// Inverse of the initial orientation difference between the two bodies + Quaternion* mInitOrientationDifferenceInv; + + // -------------------- 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 FixedJointComponent { + + /// Constructor + FixedJointComponent() { + + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + FixedJointComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~FixedJointComponents() override = default; + + /// Add a component + void addComponent(Entity jointEntity, bool isSleeping, const FixedJointComponent& component); + + /// Return a pointer to a given joint + FixedJoint* getJoint(Entity jointEntity) const; + + /// Set the joint pointer to a given joint + void setJoint(Entity jointEntity, FixedJoint* 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 translation impulse + Vector3& getImpulseTranslation(Entity jointEntity); + + /// Set the translation impulse + void setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation); + + /// Return the translation impulse + Vector3& getImpulseRotation(Entity jointEntity); + + /// Set the translation impulse + void setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation); + + /// Return the translation inverse mass matrix of the constraint + Matrix3x3& getInverseMassMatrixTranslation(Entity jointEntity); + + /// Set the translation inverse mass matrix of the constraint + void setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix); + + /// Return the rotation inverse mass matrix of the constraint + Matrix3x3& getInverseMassMatrixRotation(Entity jointEntity); + + /// Set the rotation inverse mass matrix of the constraint + void setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix); + + /// Return the translation bias + Vector3& getBiasTranslation(Entity jointEntity); + + /// Set the translation impulse + void setBiasTranslation(Entity jointEntity, const Vector3& impulseTranslation); + + /// Return the rotation bias + Vector3& getBiasRotation(Entity jointEntity); + + /// Set the rotation impulse + void setBiasRotation(Entity jointEntity, const Vector3 &impulseRotation); + + /// Return the initial orientation difference + Quaternion& getInitOrientationDifferenceInv(Entity jointEntity); + + /// Set the rotation impulse + void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv); + + // -------------------- Friendship -------------------- // + + friend class BroadPhaseSystem; +}; + +// Return a pointer to a given joint +inline FixedJoint* FixedJointComponents::getJoint(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mJoints[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the joint pointer to a given joint +inline void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* 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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mI2[mMapEntityToComponentIndex[jointEntity]] = i2; +} + +// Return the translation impulse +inline Vector3& FixedJointComponents::getImpulseTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void FixedJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation impulse +inline Vector3& FixedJointComponents::getImpulseRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void FixedJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation inverse mass matrix of the constraint +inline Matrix3x3& FixedJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + + +// Set the translation inverse mass matrix of the constraint +inline void FixedJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the rotation inverse mass matrix of the constraint +inline Matrix3x3& FixedJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation inverse mass matrix of the constraint +inline void FixedJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the translation bias +inline Vector3& FixedJointComponents::getBiasTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void FixedJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the rotation bias +inline Vector3& FixedJointComponents::getBiasRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void FixedJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; +} + +// Return the initial orientation difference +inline Quaternion& FixedJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void FixedJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; +} + +} + +#endif diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index afb84b7f..82b61b80 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -36,14 +36,14 @@ const decimal FixedJoint::BETA = decimal(0.2); // Constructor FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo) - : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { + : Joint(entity, world, jointInfo) { // Compute the local-space anchor point for each body const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + mWorld.mFixedJointsComponents.setLocalAnchoirPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mFixedJointsComponents.setLocalAnchoirPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); // Store inverse of initial rotation from body 1 to body 2 in body 1 space: // @@ -56,7 +56,7 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo // q20 = initial orientation of body 2 // q10 = initial orientation of body 1 // r0 = initial rotation rotation from body 1 to body 2 - mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); + mWorld.mFixedJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation()); } // Initialize before solving the constraint @@ -77,16 +77,22 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat const Quaternion& orientationBody2 = body2->getTransform().getOrientation(); // Get the inertia tensor of bodies - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mFixedJointsComponents.setI1(mEntity, body2->getInertiaTensorInverseWorld()); + + const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + mWorld.mFixedJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity)); + mWorld.mFixedJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mFixedJointsComponents.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); // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); @@ -95,45 +101,52 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat 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 for the 3 translation constraints - mInverseMassMatrixTranslation.setToZero(); + Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); + mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse()); } // Compute the bias "b" of the constraint for the 3 translation constraints - decimal biasFactor = (BETA / constraintSolverData.timeStep); - mBiasTranslation.setToZero(); + const decimal biasFactor = (BETA / constraintSolverData.timeStep); + Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity); + biasTranslation.setToZero(); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + mWorld.mFixedJointsComponents.setBiasTranslation(mEntity, biasFactor * (x2 + r2World - x1 - r1World)); } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotation = mI1 + mI2; + Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); + inverseMassMatrixRotation = i1 + i2; if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse()); } // Compute the bias "b" for the 3 rotation constraints - mBiasRotation.setToZero(); + Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity); + biasRotation.setToZero(); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); - mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV(); + const Quaternion qError = orientationBody2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse(); + mWorld.mFixedJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV()); } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { + Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity); + Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity); + // Reset the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); + impulseTranslation.setToZero(); + impulseRotation.setToZero(); } } @@ -157,26 +170,35 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity); + const Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity); + + const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); + // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + Vector3 linearImpulseBody1 = -impulseTranslation; + Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World); // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 - angularImpulseBody1 += -mImpulseRotation; + angularImpulseBody1 += -impulseRotation; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World); // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 - angularImpulseBody2 += mImpulseRotation; + angularImpulseBody2 += impulseRotation; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mImpulseTranslation; - w2 += mI2 * angularImpulseBody2; + v2 += inverseMassBody2 * impulseTranslation; + w2 += i2 * angularImpulseBody2; } // Solve the velocity constraint @@ -199,48 +221,59 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); + // --------------- Translation Constraints --------------- // // Compute J*v for the 3 translation constraints - const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); + + const Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity); + const Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = mInverseMassMatrixTranslation * - (-JvTranslation - mBiasTranslation); - mImpulseTranslation += deltaLambda; + const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - biasTranslation); + mWorld.mFixedJointsComponents.setImpulseTranslation(mEntity, mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) + deltaLambda); // Compute the impulse P=J^T * lambda for body 1 const Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + Vector3 angularImpulseBody1 = deltaLambda.cross(r1World); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); + const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambda; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; // --------------- Rotation Constraints --------------- // // Compute J*v for the 3 rotation constraints const Vector3 JvRotation = w2 - w1; + const Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity); + const Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); + // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation); - mImpulseRotation += deltaLambda2; + Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation); + mWorld.mFixedJointsComponents.setImpulseRotation(mEntity, mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) + deltaLambda2); // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Apply the impulse to the body 2 - w2 += mI2 * deltaLambda2; + w2 += i2 * deltaLambda2; } // Solve the position constraint (for position error correction) @@ -268,17 +301,23 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); + + const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); + // Recompute the inverse inertia tensors - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mFixedJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + mWorld.mFixedJointsComponents.setR1World(mEntity, q1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity)); + mWorld.mFixedJointsComponents.setR2World(mEntity, q2 * mWorld.mFixedJointsComponents.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); // --------------- Translation Constraints --------------- // @@ -287,27 +326,28 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrixTranslation.setToZero(); + skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose(); + Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); + mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse()); } // Compute position error for the 3 translation constraints - const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World; + const Vector3 errorTranslation = x2 + r2World - x1 - r1World; // Compute the Lagrange multiplier lambda - const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation); + const Vector3 lambdaTranslation = inverseMassMatrixTranslation * (-errorTranslation); // Compute the impulse of body 1 Vector3 linearImpulseBody1 = -lambdaTranslation; - Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World); + Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); // Compute the pseudo velocity of body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mI1 * angularImpulseBody1; + Vector3 w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -315,11 +355,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q1.normalize(); // Compute the impulse of body 2 - Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World); + Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World); // Compute the pseudo velocity of body 2 const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = mI2 * angularImpulseBody2; + Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -330,10 +370,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotation = mI1 + mI2; + Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); + inverseMassMatrixRotation = i1 + i2; if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); + mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, inverseMassMatrixRotation.getInverse()); } // Calculate difference in rotation @@ -351,7 +392,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // q1 = current rotation of body 1 // q2 = current rotation of body 2 // qError = error that needs to be reduced to zero - Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse(); + Quaternion qError = q2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * q1.getInverse(); // A quaternion can be seen as: // @@ -365,20 +406,20 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + Vector3 lambdaRotation = inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -lambdaRotation; // Compute the pseudo velocity of body 1 - w1 = mI1 * angularImpulseBody1; + w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); // Compute the pseudo velocity of body 2 - w2 = mI2 * lambdaRotation; + w2 = i2 * lambdaRotation; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -390,3 +431,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } +// Return a string representation +std::string FixedJoint::to_string() const { + return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() + + ", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() + + ", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + + "}"; +} + diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index 150cb218..57b302cf 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -73,47 +73,6 @@ class FixedJoint : public Joint { // Beta value for the bias factor of position correction static const decimal BETA; - // -------------------- 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; - - /// Accumulated impulse for the 3 translation constraints - Vector3 mImpulseTranslation; - - /// Accumulate impulse for the 3 rotation constraints - Vector3 mImpulseRotation; - - /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) - Matrix3x3 mInverseMassMatrixTranslation; - - /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) - Matrix3x3 mInverseMassMatrixRotation; - - /// Bias vector for the 3 translation constraints - Vector3 mBiasTranslation; - - /// Bias vector for the 3 rotation constraints - Vector3 mBiasRotation; - - /// Inverse of the initial orientation difference between the two bodies - Quaternion mInitOrientationDifferenceInv; - // -------------------- Methods -------------------- // /// Return the number of bytes used by the joint @@ -156,14 +115,6 @@ inline size_t FixedJoint::getSizeInBytes() const { return sizeof(FixedJoint); } -// Return a string representation -inline std::string FixedJoint::to_string() const { - return "FixedJoint{ localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() + - ", localAnchorPointBody2=" + mLocalAnchorPointBody2.to_string() + - ", initOrientationDifferenceInv=" + mInitOrientationDifferenceInv.to_string() + - "}"; -} - } #endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 39fc3531..b12ed42f 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -41,6 +41,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()), + mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), @@ -261,6 +262,12 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) { // TODO : Make sure we notify all the components here ... mJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); + if (mBallAndSocketJointsComponents.hasComponent(jointEntity)) { + mBallAndSocketJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); + } + if (mFixedJointsComponents.hasComponent(jointEntity)) { + mFixedJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); + } } // Return true if two bodies overlap diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 007c3a9e..ab7fc92f 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,6 +39,7 @@ #include "components/ProxyShapeComponents.h" #include "components/JointComponents.h" #include "components/BallAndSocketJointComponents.h" +#include "components/FixedJointComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -96,6 +97,9 @@ class CollisionWorld { /// Ball And Socket joints Components BallAndSocketJointComponents mBallAndSocketJointsComponents; + /// Fixed joints Components + FixedJointComponents mFixedJointsComponents; + /// Reference to the collision detection CollisionDetectionSystem mCollisionDetection; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 1a67ca18..a784a82d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -368,10 +368,19 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { + // Create a BallAndSocketJoint component + FixedJointComponents::FixedJointComponent fixedJointComponent; + mFixedJointsComponents.addComponent(entity, isJointDisabled, fixedJointComponent); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) FixedJoint(entity, *this, info); + FixedJoint* joint = new (allocatedMemory) FixedJoint(entity, *this, info); + + newJoint = joint; + + mFixedJointsComponents.setJoint(entity, joint); + break; } @@ -467,6 +476,9 @@ void DynamicsWorld::addJointToBody(Joint* joint) { sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, body1->mJointsList); + RigidBody* test1 = joint->getBody1(); + RigidBody* test2 = joint->getBody2(); + body1->mJointsList = jointListElement1; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,