From 0c0ff46d349e33ee4361f74c51546058636963ff Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 13 Sep 2019 07:15:48 +0200 Subject: [PATCH] Add SliderJointComponents class --- CMakeLists.txt | 2 + src/components/HingeJointComponents.cpp | 6 +- src/components/SliderJointComponents.cpp | 408 +++++++++++ src/components/SliderJointComponents.h | 863 +++++++++++++++++++++++ src/constraint/SliderJoint.cpp | 184 +++-- src/constraint/SliderJoint.h | 52 +- src/engine/CollisionWorld.cpp | 4 + src/engine/CollisionWorld.h | 4 + src/engine/DynamicsWorld.cpp | 29 +- src/engine/DynamicsWorld.h | 1 - 10 files changed, 1421 insertions(+), 132 deletions(-) create mode 100644 src/components/SliderJointComponents.cpp create mode 100644 src/components/SliderJointComponents.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e771c0f..d16403b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,6 +150,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/BallAndSocketJointComponents.h" "src/components/FixedJointComponents.h" "src/components/HingeJointComponents.h" + "src/components/SliderJointComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -246,6 +247,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/BallAndSocketJointComponents.cpp" "src/components/FixedJointComponents.cpp" "src/components/HingeJointComponents.cpp" + "src/components/SliderJointComponents.cpp" "src/collision/CollisionCallback.cpp" "src/collision/OverlapCallback.cpp" "src/mathematics/mathematics_functions.cpp" diff --git a/src/components/HingeJointComponents.cpp b/src/components/HingeJointComponents.cpp index b3880e63..6a71ba9c 100644 --- a/src/components/HingeJointComponents.cpp +++ b/src/components/HingeJointComponents.cpp @@ -197,11 +197,11 @@ void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, con new (mI1 + index) Matrix3x3(); new (mI2 + index) Matrix3x3(); new (mImpulseTranslation + index) Vector3(0, 0, 0); - new (mImpulseRotation + index) Vector3(0, 0, 0); + new (mImpulseRotation + index) Vector2(0, 0); new (mInverseMassMatrixTranslation + index) Matrix3x3(); - new (mInverseMassMatrixRotation + index) Matrix3x3(); + new (mInverseMassMatrixRotation + index) Matrix2x2(); new (mBiasTranslation + index) Vector3(0, 0, 0); - new (mBiasRotation + index) Vector3(0, 0, 0); + new (mBiasRotation + index) Vector2(0, 0); new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0); new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0); new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0); diff --git a/src/components/SliderJointComponents.cpp b/src/components/SliderJointComponents.cpp new file mode 100644 index 00000000..d7bdc751 --- /dev/null +++ b/src/components/SliderJointComponents.cpp @@ -0,0 +1,408 @@ +/******************************************************************************** +* 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 "SliderJointComponents.h" +#include "engine/EntityManager.h" +#include "mathematics/Matrix3x3.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(SliderJoint*) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector2) + + sizeof(Vector3) + sizeof(Matrix2x2) + sizeof(Matrix3x3) + + sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion)/* + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) + + sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal)*/) { + + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_COMPONENTS); +} + +// Allocate memory for a given number of components +void SliderJointComponents::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); + SliderJoint** newJoints = reinterpret_cast(newJointEntities + nbComponentsToAllocate); + Vector3* newLocalAnchorPointBody1 = reinterpret_cast(newJoints + nbComponentsToAllocate); + Vector3* newLocalAnchorPointBody2 = reinterpret_cast(newLocalAnchorPointBody1 + nbComponentsToAllocate); + Matrix3x3* newI1 = reinterpret_cast(newLocalAnchorPointBody2 + nbComponentsToAllocate); + Matrix3x3* newI2 = reinterpret_cast(newI1 + nbComponentsToAllocate); + Vector2* newImpulseTranslation = reinterpret_cast(newI2 + nbComponentsToAllocate); + Vector3* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); + Matrix2x2* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); + Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); + Vector2* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); + Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); + Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); + /* + Vector3* newHingeLocalAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); + Vector3* newHingeLocalAxisBody2 = reinterpret_cast(newHingeLocalAxisBody1 + nbComponentsToAllocate); + Vector3* newA1 = reinterpret_cast(newHingeLocalAxisBody2 + nbComponentsToAllocate); + Vector3* newB2CrossA1 = reinterpret_cast(newA1 + nbComponentsToAllocate); + Vector3* newC2CrossA1 = reinterpret_cast(newB2CrossA1 + nbComponentsToAllocate); + decimal* newImpulseLowerLimit = reinterpret_cast(newC2CrossA1 + nbComponentsToAllocate); + decimal* newImpulseUpperLimit = reinterpret_cast(newImpulseLowerLimit + nbComponentsToAllocate); + decimal* newImpulseMotor = reinterpret_cast(newImpulseUpperLimit + nbComponentsToAllocate); + decimal* newInverseMassMatrixLimitMotor = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); + decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimitMotor + nbComponentsToAllocate); + decimal* newBLowerLimit = reinterpret_cast(newInverseMassMatrixMotor + nbComponentsToAllocate); + decimal* newBUpperLimit = reinterpret_cast(newBLowerLimit + nbComponentsToAllocate); + bool* newIsLimitEnabled = reinterpret_cast(newBUpperLimit + nbComponentsToAllocate); + bool* newIsMotorEnabled = reinterpret_cast(newIsLimitEnabled + nbComponentsToAllocate); + decimal* newLowerLimit = reinterpret_cast(newIsMotorEnabled + nbComponentsToAllocate); + decimal* newUpperLimit = reinterpret_cast(newLowerLimit + nbComponentsToAllocate); + bool* newIsLowerLimitViolated = reinterpret_cast(newUpperLimit + nbComponentsToAllocate); + bool* newIsUpperLimitViolated = reinterpret_cast(newIsLowerLimitViolated + nbComponentsToAllocate); + decimal* newMotorSpeed = reinterpret_cast(newIsUpperLimitViolated + nbComponentsToAllocate); + decimal* newMaxMotorTorque = reinterpret_cast(newMotorSpeed + 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(SliderJoint*)); + memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3)); + memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3)); + memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3)); + memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3)); + memcpy(newImpulseTranslation, mImpulseTranslation, mNbComponents * sizeof(Vector2)); + memcpy(newImpulseRotation, mImpulseRotation, mNbComponents * sizeof(Vector3)); + memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix2x2)); + memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix3x3)); + memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector2)); + memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3)); + memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion)); + /* + memcpy(newHingeLocalAxisBody1, mHingeLocalAxisBody1, mNbComponents * sizeof(Vector3)); + memcpy(newHingeLocalAxisBody2, mHingeLocalAxisBody2, mNbComponents * sizeof(Vector3)); + memcpy(newA1, mA1, mNbComponents * sizeof(Vector3)); + memcpy(newB2CrossA1, mB2CrossA1, mNbComponents * sizeof(Vector3)); + memcpy(newC2CrossA1, mC2CrossA1, mNbComponents * sizeof(Vector3)); + memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal)); + memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal)); + memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal)); + memcpy(newInverseMassMatrixLimitMotor, mInverseMassMatrixLimitMotor, mNbComponents * sizeof(decimal)); + memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal)); + memcpy(newBLowerLimit, mBLowerLimit, mNbComponents * sizeof(decimal)); + memcpy(newBUpperLimit, mBUpperLimit, mNbComponents * sizeof(decimal)); + memcpy(newIsLimitEnabled, mIsLimitEnabled, mNbComponents * sizeof(bool)); + memcpy(newIsMotorEnabled, mIsMotorEnabled, mNbComponents * sizeof(bool)); + memcpy(newLowerLimit, mLowerLimit, mNbComponents * sizeof(decimal)); + memcpy(newUpperLimit, mUpperLimit, mNbComponents * sizeof(decimal)); + memcpy(newIsLowerLimitViolated, mIsLowerLimitViolated, mNbComponents * sizeof(bool)); + memcpy(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool)); + memcpy(newMotorSpeed, mMotorSpeed, mNbComponents * sizeof(decimal)); + memcpy(newMaxMotorTorque, mMaxMotorTorque, mNbComponents * sizeof(decimal)); + */ + + // Deallocate previous memory + mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); + } + + mBuffer = newBuffer; + mJointEntities = newJointEntities; + mJoints = newJoints; + mNbAllocatedComponents = nbComponentsToAllocate; + mLocalAnchorPointBody1 = newLocalAnchorPointBody1; + mLocalAnchorPointBody2 = newLocalAnchorPointBody2; + mI1 = newI1; + mI2 = newI2; + mImpulseTranslation = newImpulseTranslation; + mImpulseRotation = newImpulseRotation; + mInverseMassMatrixTranslation = newInverseMassMatrixTranslation; + mInverseMassMatrixRotation = newInverseMassMatrixRotation; + mBiasTranslation = newBiasTranslation; + mBiasRotation = newBiasRotation; + mInitOrientationDifferenceInv = newInitOrientationDifferenceInv; + /* + mHingeLocalAxisBody1 = newHingeLocalAxisBody1; + mHingeLocalAxisBody2 = newHingeLocalAxisBody2; + mA1 = newA1; + mB2CrossA1 = newB2CrossA1; + mC2CrossA1 = newC2CrossA1; + mImpulseLowerLimit = newImpulseLowerLimit; + mImpulseUpperLimit = newImpulseUpperLimit; + mImpulseMotor = newImpulseMotor; + mInverseMassMatrixLimitMotor = newInverseMassMatrixLimitMotor; + mInverseMassMatrixMotor = newInverseMassMatrixMotor; + mBLowerLimit = newBLowerLimit; + mBUpperLimit = newBUpperLimit; + mIsLimitEnabled = newIsLimitEnabled; + mIsMotorEnabled = newIsMotorEnabled; + mLowerLimit = newLowerLimit; + mUpperLimit = newUpperLimit; + mIsLowerLimitViolated = newIsLowerLimitViolated; + mIsUpperLimitViolated = newIsUpperLimitViolated; + mMotorSpeed = newMotorSpeed; + mMaxMotorTorque = newMaxMotorTorque; + */ +} + +// Add a component +void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, const SliderJointComponent& 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 (mI1 + index) Matrix3x3(); + new (mI2 + index) Matrix3x3(); + new (mImpulseTranslation + index) Vector2(0, 0); + new (mImpulseRotation + index) Vector3(0, 0, 0); + new (mInverseMassMatrixTranslation + index) Matrix2x2(); + new (mInverseMassMatrixRotation + index) Matrix3x3(); + new (mBiasTranslation + index) Vector2(0, 0); + new (mBiasRotation + index) Vector3(0, 0, 0); + new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0); + /* + new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0); + new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0); + new (mA1 + index) Vector3(0, 0, 0); + new (mB2CrossA1 + index) Vector3(0, 0, 0); + new (mC2CrossA1 + index) Vector3(0, 0, 0); + mImpulseLowerLimit[index] = decimal(0.0); + mImpulseUpperLimit[index] = decimal(0.0); + mInverseMassMatrixLimitMotor[index] = decimal(0.0); + mInverseMassMatrixMotor[index] = decimal(0.0); + mBLowerLimit[index] = decimal(0.0); + mBUpperLimit[index] = decimal(0.0); + mIsLimitEnabled[index] = component.isLimitEnabled; + mIsMotorEnabled[index] = component.isMotorEnabled; + mLowerLimit[index] = component.lowerLimit; + mUpperLimit[index] = component.upperLimit; + mIsLowerLimitViolated[index] = false; + mIsUpperLimitViolated[index] = false; + mMotorSpeed[index] = component.motorSpeed; + mMaxMotorTorque[index] = component.maxMotorTorque; + */ + + // 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 SliderJointComponents::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 (mI1 + destIndex) Matrix3x3(mI1[srcIndex]); + new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]); + new (mImpulseTranslation + destIndex) Vector2(mImpulseTranslation[srcIndex]); + new (mImpulseRotation + destIndex) Vector3(mImpulseRotation[srcIndex]); + new (mInverseMassMatrixTranslation + destIndex) Matrix2x2(mInverseMassMatrixTranslation[srcIndex]); + new (mInverseMassMatrixRotation + destIndex) Matrix3x3(mInverseMassMatrixRotation[srcIndex]); + new (mBiasTranslation + destIndex) Vector2(mBiasTranslation[srcIndex]); + new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]); + new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]); + /* + new (mHingeLocalAxisBody1 + destIndex) Vector3(mHingeLocalAxisBody1[srcIndex]); + new (mHingeLocalAxisBody2 + destIndex) Vector3(mHingeLocalAxisBody2[srcIndex]); + new (mA1 + destIndex) Vector3(mA1[srcIndex]); + new (mB2CrossA1 + destIndex) Vector3(mB2CrossA1[srcIndex]); + new (mC2CrossA1 + destIndex) Vector3(mC2CrossA1[srcIndex]); + mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex]; + mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex]; + mImpulseMotor[destIndex] = mImpulseMotor[srcIndex]; + mInverseMassMatrixLimitMotor[destIndex] = mInverseMassMatrixLimitMotor[srcIndex]; + mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex]; + mBLowerLimit[destIndex] = mBLowerLimit[srcIndex]; + mBUpperLimit[destIndex] = mBUpperLimit[srcIndex]; + mIsLimitEnabled[destIndex] = mIsLimitEnabled[srcIndex]; + mIsMotorEnabled[destIndex] = mIsMotorEnabled[srcIndex]; + mLowerLimit[destIndex] = mLowerLimit[srcIndex]; + mUpperLimit[destIndex] = mUpperLimit[srcIndex]; + mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex]; + mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[srcIndex]; + mMotorSpeed[destIndex] = mMotorSpeed[srcIndex]; + mMaxMotorTorque[destIndex] = mMaxMotorTorque[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 SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity jointEntity1(mJointEntities[index1]); + SliderJoint* joint1 = mJoints[index1]; + Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]); + Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]); + Matrix3x3 i11(mI1[index1]); + Matrix3x3 i21(mI2[index1]); + Vector2 impulseTranslation1(mImpulseTranslation[index1]); + Vector3 impulseRotation1(mImpulseRotation[index1]); + Matrix2x2 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]); + Matrix3x3 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]); + Vector2 biasTranslation1(mBiasTranslation[index1]); + Vector3 biasRotation1(mBiasRotation[index1]); + Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]); + /* + Vector3 hingeLocalAxisBody1(mHingeLocalAxisBody1[index1]); + Vector3 hingeLocalAxisBody2(mHingeLocalAxisBody2[index1]); + Vector3 a1(mA1[index1]); + Vector3 b2CrossA1(mB2CrossA1[index1]); + Vector3 c2CrossA1(mC2CrossA1[index1]); + decimal impulseLowerLimit(mImpulseLowerLimit[index1]); + decimal impulseUpperLimit(mImpulseUpperLimit[index1]); + decimal impulseMotor(mImpulseMotor[index1]); + decimal inverseMassMatrixLimitMotor(mInverseMassMatrixLimitMotor[index1]); + decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]); + decimal bLowerLimit(mBLowerLimit[index1]); + decimal bUpperLimit(mUpperLimit[index1]); + bool isLimitEnabled(mIsLimitEnabled[index1]); + bool isMotorEnabled(mIsMotorEnabled[index1]); + decimal lowerLimit(mLowerLimit[index1]); + decimal upperLimit(mUpperLimit[index1]); + bool isLowerLimitViolated(mIsLowerLimitViolated[index1]); + bool isUpperLimitViolated(mIsUpperLimitViolated[index1]); + decimal motorSpeed(mMotorSpeed[index1]); + decimal maxMotorTorque(mMaxMotorTorque[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 (mI1 + index2) Matrix3x3(i11); + new (mI2 + index2) Matrix3x3(i21); + new (mImpulseTranslation + index2) Vector2(impulseTranslation1); + new (mImpulseRotation + index2) Vector3(impulseRotation1); + new (mInverseMassMatrixTranslation + index2) Matrix2x2(inverseMassMatrixTranslation1); + new (mInverseMassMatrixRotation + index2) Matrix3x3(inverseMassMatrixRotation1); + new (mBiasTranslation + index2) Vector2(biasTranslation1); + new (mBiasRotation + index2) Vector3(biasRotation1); + new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); + /* + new (mHingeLocalAxisBody1 + index2) Vector3(hingeLocalAxisBody1); + new (mHingeLocalAxisBody2 + index2) Vector3(hingeLocalAxisBody2); + new (mA1 + index2) Vector3(a1); + new (mB2CrossA1 + index2) Vector3(b2CrossA1); + new (mC2CrossA1 + index2) Vector3(c2CrossA1); + mImpulseLowerLimit[index2] = impulseLowerLimit; + mImpulseUpperLimit[index2] = impulseUpperLimit; + mImpulseMotor[index2] = impulseMotor; + mInverseMassMatrixLimitMotor[index2] = inverseMassMatrixLimitMotor; + mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor; + mBLowerLimit[index2] = bLowerLimit; + mBUpperLimit[index2] = bUpperLimit; + mIsLimitEnabled[index2] = isLimitEnabled; + mIsMotorEnabled[index2] = isMotorEnabled; + mLowerLimit[index2] = lowerLimit; + mUpperLimit[index2] = upperLimit; + mIsLowerLimitViolated[index2] = isLowerLimitViolated; + mIsUpperLimitViolated[index2] = isUpperLimitViolated; + mMotorSpeed[index2] = motorSpeed; + mMaxMotorTorque[index2] = maxMotorTorque; + */ + + // 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 SliderJointComponents::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(); + mI1[index].~Matrix3x3(); + mI2[index].~Matrix3x3(); + mImpulseTranslation[index].~Vector2(); + mImpulseRotation[index].~Vector3(); + mInverseMassMatrixTranslation[index].~Matrix2x2(); + mInverseMassMatrixRotation[index].~Matrix3x3(); + mBiasTranslation[index].~Vector2(); + mBiasRotation[index].~Vector3(); + mInitOrientationDifferenceInv[index].~Quaternion(); + /* + mHingeLocalAxisBody1[index].~Vector3(); + mHingeLocalAxisBody2[index].~Vector3(); + mA1[index].~Vector3(); + mB2CrossA1[index].~Vector3(); + mC2CrossA1[index].~Vector3(); + */ +} diff --git a/src/components/SliderJointComponents.h b/src/components/SliderJointComponents.h new file mode 100644 index 00000000..848330a2 --- /dev/null +++ b/src/components/SliderJointComponents.h @@ -0,0 +1,863 @@ +/******************************************************************************** +* 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_SLIDER_JOINT_COMPONENTS_H +#define REACTPHYSICS3D_SLIDER_JOINT_COMPONENTS_H + +// Libraries +#include "mathematics/Transform.h" +#include "mathematics/Matrix3x3.h" +#include "mathematics/Matrix2x2.h" +#include "engine/Entity.h" +#include "components/Components.h" +#include "containers/Map.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class declarations +class MemoryAllocator; +class EntityManager; +class SliderJoint; +enum class JointType; + +// Class SliderJointComponents +/** + * This class represent the component of the ECS with data for the SliderJoint. + */ +class SliderJointComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of joint entities + Entity* mJointEntities; + + /// Array of pointers to the joints + SliderJoint** 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; + + /// 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 + Vector2* mImpulseTranslation; + + /// Accumulate impulse for the 3 rotation constraints + Vector3* mImpulseRotation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix) + Matrix2x2* mInverseMassMatrixTranslation; + + /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix) + Matrix3x3* mInverseMassMatrixRotation; + + /// Bias vector for the 3 translation constraints + Vector2* mBiasTranslation; + + /// Bias vector for the 3 rotation constraints + Vector3* mBiasRotation; + + /// Inverse of the initial orientation difference between the two bodies + Quaternion* mInitOrientationDifferenceInv; + + /* + /// Hinge rotation axis (in local-space coordinates of body 1) + Vector3* mHingeLocalAxisBody1; + + /// Hinge rotation axis (in local-space coordiantes of body 2) + Vector3* mHingeLocalAxisBody2; + + /// Hinge rotation axis (in world-space coordinates) computed from body 1 + Vector3* mA1; + + /// Cross product of vector b2 and a1 + Vector3* mB2CrossA1; + + /// Cross product of vector c2 and a1; + Vector3* mC2CrossA1; + + /// Accumulated impulse for the lower limit constraint + decimal* mImpulseLowerLimit; + + /// Accumulated impulse for the upper limit constraint + decimal* mImpulseUpperLimit; + + /// Accumulated impulse for the motor constraint; + decimal* mImpulseMotor; + + /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + decimal* mInverseMassMatrixLimitMotor; + + /// Inverse of mass matrix K=JM^-1J^t for the motor + decimal* mInverseMassMatrixMotor; + + /// Bias of the lower limit constraint + decimal* mBLowerLimit; + + /// Bias of the upper limit constraint + decimal* mBUpperLimit; + + /// True if the joint limits are enabled + bool* mIsLimitEnabled; + + /// True if the motor of the joint in enabled + bool* mIsMotorEnabled; + + /// Lower limit (minimum allowed rotation angle in radian) + decimal* mLowerLimit; + + /// Upper limit (maximum translation distance) + decimal* mUpperLimit; + + /// True if the lower limit is violated + bool* mIsLowerLimitViolated; + + /// True if the upper limit is violated + bool* mIsUpperLimitViolated; + + /// Motor speed (in rad/s) + decimal* mMotorSpeed; + + /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + decimal* mMaxMotorTorque; + + */ + + // -------------------- 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 SliderJointComponent { + + bool isLimitEnabled; + bool isMotorEnabled; + decimal lowerLimit; + decimal upperLimit; + decimal motorSpeed; + decimal maxMotorTorque; + + // TODO : Delete this + SliderJointComponent() { + + } + + /// Constructor + SliderJointComponent(bool isLimitEnabled, bool isMotorEnabled, decimal lowerLimit, decimal upperLimit, + decimal motorSpeed, decimal maxMotorTorque) + : isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit), + motorSpeed(motorSpeed), maxMotorTorque(maxMotorTorque) { + + } + }; + + // -------------------- Methods -------------------- // + + /// Constructor + SliderJointComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~SliderJointComponents() override = default; + + /// Add a component + void addComponent(Entity jointEntity, bool isSleeping, const SliderJointComponent& component); + + /// Return a pointer to a given joint + SliderJoint* getJoint(Entity jointEntity) const; + + /// Set the joint pointer to a given joint + void setJoint(Entity jointEntity, SliderJoint* joint) const; + + /// Return the local anchor point of body 1 for a given joint + const Vector3& getLocalAnchorPointBody1(Entity jointEntity) const; + + /// Set the local anchor point of body 1 for a given joint + void setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1); + + /// Return the local anchor point of body 2 for a given joint + const Vector3& getLocalAnchorPointBody2(Entity jointEntity) const; + + /// Set the local anchor point of body 2 for a given joint + void setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2); + + /// 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 + Vector2& getImpulseTranslation(Entity jointEntity); + + /// Set the translation impulse + void setImpulseTranslation(Entity jointEntity, const Vector2& 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 + Matrix2x2& getInverseMassMatrixTranslation(Entity jointEntity); + + /// Set the translation inverse mass matrix of the constraint + void setInverseMassMatrixTranslation(Entity jointEntity, const Matrix2x2& 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 + Vector2& getBiasTranslation(Entity jointEntity); + + /// Set the translation impulse + void setBiasTranslation(Entity jointEntity, const Vector2& 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); + + /* + /// Return the hinge rotation axis (in local-space coordinates of body 1) + Vector3& getHingeLocalAxisBody1(Entity jointEntity); + + /// Set the hinge rotation axis (in local-space coordinates of body 1) + void setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1); + + /// Return the hinge rotation axis (in local-space coordiantes of body 2) + Vector3& getHingeLocalAxisBody2(Entity jointEntity); + + /// Set the hinge rotation axis (in local-space coordiantes of body 2) + void setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2); + + /// Return the hinge rotation axis (in world-space coordinates) computed from body 1 + Vector3& getA1(Entity jointEntity); + + /// Set the hinge rotation axis (in world-space coordinates) computed from body 1 + void setA1(Entity jointEntity, const Vector3& a1); + + /// Return the cross product of vector b2 and a1 + Vector3& getB2CrossA1(Entity jointEntity); + + /// Set the cross product of vector b2 and a1 + void setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1); + + /// Return the cross product of vector c2 and a1; + Vector3& getC2CrossA1(Entity jointEntity); + + /// Set the cross product of vector c2 and a1; + void setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1); + + /// Return the accumulated impulse for the lower limit constraint + decimal getImpulseLowerLimit(Entity jointEntity) const; + + /// Set the accumulated impulse for the lower limit constraint + void setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit); + + /// Return the accumulated impulse for the upper limit constraint + decimal getImpulseUpperLimit(Entity jointEntity) const; + + /// Set the accumulated impulse for the upper limit constraint + void setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const; + + /// Return the accumulated impulse for the motor constraint; + decimal getImpulseMotor(Entity jointEntity) const; + + /// Set the accumulated impulse for the motor constraint; + void setImpulseMotor(Entity jointEntity, decimal impulseMotor); + + /// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + decimal getInverseMassMatrixLimitMotor(Entity jointEntity) const; + + /// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) + void setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor); + + /// Return the inverse of mass matrix K=JM^-1J^t for the motor + decimal getInverseMassMatrixMotor(Entity jointEntity); + + /// Set the inverse of mass matrix K=JM^-1J^t for the motor + void setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor); + + /// Return the bias of the lower limit constraint + decimal getBLowerLimit(Entity jointEntity) const; + + /// Set the bias of the lower limit constraint + void setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const; + + /// Return the bias of the upper limit constraint + decimal getBUpperLimit(Entity jointEntity) const; + + /// Set the bias of the upper limit constraint + void setBUpperLimit(Entity jointEntity, decimal bUpperLimit); + + /// Return true if the joint limits are enabled + bool getIsLimitEnabled(Entity jointEntity) const; + + /// Set to true if the joint limits are enabled + void setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled); + + /// Return true if the motor of the joint in enabled + bool getIsMotorEnabled(Entity jointEntity) const; + + /// Set to true if the motor of the joint in enabled + void setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const; + + /// Return the Lower limit (minimum allowed rotation angle in radian) + decimal getLowerLimit(Entity jointEntity) const; + + /// Set the Lower limit (minimum allowed rotation angle in radian) + void setLowerLimit(Entity jointEntity, decimal lowerLimit) const; + + /// Return the upper limit (maximum translation distance) + decimal getUpperLimit(Entity jointEntity) const; + + /// Set the upper limit (maximum translation distance) + void setUpperLimit(Entity jointEntity, decimal upperLimit); + + /// Return true if the lower limit is violated + bool getIsLowerLimitViolated(Entity jointEntity) const; + + /// Set to true if the lower limit is violated + void setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated); + + /// Return true if the upper limit is violated + bool getIsUpperLimitViolated(Entity jointEntity) const; + + /// Set to true if the upper limit is violated + void setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const; + + /// Return the motor speed (in rad/s) + decimal getMotorSpeed(Entity jointEntity) const; + + /// Set the motor speed (in rad/s) + void setMotorSpeed(Entity jointEntity, decimal motorSpeed); + + /// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + decimal getMaxMotorTorque(Entity jointEntity) const; + + /// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + void setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque); + */ + + // -------------------- Friendship -------------------- // + + friend class BroadPhaseSystem; +}; + +// Return a pointer to a given joint +inline SliderJoint* SliderJointComponents::getJoint(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mJoints[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the joint pointer to a given joint +inline void SliderJointComponents::setJoint(Entity jointEntity, SliderJoint* 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& SliderJointComponents::getLocalAnchorPointBody1(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 SliderJointComponents::setLocalAnchorPointBody1(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& SliderJointComponents::getLocalAnchorPointBody2(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 SliderJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; +} + +// Return the inertia tensor of body 1 (in world-space coordinates) +inline const Matrix3x3& SliderJointComponents::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 SliderJointComponents::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& SliderJointComponents::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 SliderJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mI2[mMapEntityToComponentIndex[jointEntity]] = i2; +} + +// Return the translation impulse +inline Vector2& SliderJointComponents::getImpulseTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void SliderJointComponents::setImpulseTranslation(Entity jointEntity, const Vector2& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation impulse +inline Vector3& SliderJointComponents::getImpulseRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void SliderJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation inverse mass matrix of the constraint +inline Matrix2x2& SliderJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation inverse mass matrix of the constraint +inline void SliderJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the rotation inverse mass matrix of the constraint +inline Matrix3x3& SliderJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation inverse mass matrix of the constraint +inline void SliderJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the translation bias +inline Vector2& SliderJointComponents::getBiasTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void SliderJointComponents::setBiasTranslation(Entity jointEntity, const Vector2& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the rotation bias +inline Vector3& SliderJointComponents::getBiasRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void SliderJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; +} + +// Return the initial orientation difference +inline Quaternion& SliderJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void SliderJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; +} + +/* +// Return the hinge rotation axis (in local-space coordinates of body 1) +inline Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the hinge rotation axis (in local-space coordinates of body 1) +inline void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody1; +} + +// Return the hinge rotation axis (in local-space coordiantes of body 2) +inline Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the hinge rotation axis (in local-space coordiantes of body 2) +inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody2; +} + + +// Return the hinge rotation axis (in world-space coordinates) computed from body 1 +inline Vector3& HingeJointComponents::getA1(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mA1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the hinge rotation axis (in world-space coordinates) computed from body 1 +inline void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mA1[mMapEntityToComponentIndex[jointEntity]] = a1; +} + +// Return the cross product of vector b2 and a1 +inline Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mB2CrossA1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the cross product of vector b2 and a1 +inline void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mB2CrossA1[mMapEntityToComponentIndex[jointEntity]] = b2CrossA1; +} + +// Return the cross product of vector c2 and a1; +inline Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mC2CrossA1[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the cross product of vector c2 and a1; +inline void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mC2CrossA1[mMapEntityToComponentIndex[jointEntity]] = c2CrossA1; +} + +// Return the accumulated impulse for the lower limit constraint +inline decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the accumulated impulse for the lower limit constraint +inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit; +} + + +// Return the accumulated impulse for the upper limit constraint +inline decimal HingeJointComponents::getImpulseUpperLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the accumulated impulse for the upper limit constraint +inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit; +} + + +// Return the accumulated impulse for the motor constraint; +inline decimal HingeJointComponents::getImpulseMotor(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the accumulated impulse for the motor constraint; +inline void HingeJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor; +} + +// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) +inline decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) +inline void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor; +} + +// Return the inverse of mass matrix K=JM^-1J^t for the motor +inline decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]]; +} + +// Return the inverse of mass matrix K=JM^-1J^t for the motor +inline void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor; +} + +// Return the bias of the lower limit constraint +inline decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the bias of the lower limit constraint +inline void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit; +} + +// Return the bias of the upper limit constraint +inline decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the bias of the upper limit constraint +inline void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit; +} + +// Return true if the joint limits are enabled +inline bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set to true if the joint limits are enabled +inline void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled; +} + +// Return true if the motor of the joint in enabled +inline bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set to true if the motor of the joint in enabled +inline void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled; +} + +// Return the Lower limit (minimum allowed rotation angle in radian) +inline decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mLowerLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the Lower limit (minimum allowed rotation angle in radian) +inline void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit; +} + +// Return the upper limit (maximum translation distance) +inline decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mUpperLimit[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the upper limit (maximum translation distance) +inline void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit; +} + +// Return true if the lower limit is violated +inline bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set to true if the lower limit is violated +inline void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated; +} + +// Return true if the upper limit is violated +inline bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set to true if the upper limit is violated +inline void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated; +} + +// Return the motor speed (in rad/s) +inline decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the motor speed (in rad/s) +inline void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed; +} + +// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed +inline decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed +inline void HingeJointComponents::setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]] = maxMotorTorque; +} +*/ + +} + +#endif diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 6fd63529..2a05079c 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -36,8 +36,7 @@ const decimal SliderJoint::BETA = decimal(0.2); // Constructor SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo) - : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), - mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), + : Joint(entity, world, jointInfo), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minTranslationLimit), mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), @@ -51,8 +50,8 @@ SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointI // 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.mSliderJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mSliderJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); // Store inverse of initial rotation from body 1 to body 2 in body 1 space: // @@ -66,7 +65,7 @@ SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointI // q10 = initial orientation of body 1 // r0 = initial rotation rotation from body 1 to body 2 // TODO : Do not compute the inverse here, it has already been computed above - mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); + mWorld.mSliderJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation()); // Compute the slider axis in local-space of body 1 // TODO : Do not compute the inverse here, it has already been computed above @@ -93,12 +92,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); // Get the inertia tensor of bodies - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Vector from body center to the anchor point - mR1 = orientationBody1 * mLocalAnchorPointBody1; - mR2 = orientationBody2 * mLocalAnchorPointBody2; + mR1 = orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); + mR2 = orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); // Compute the vector u (difference between anchor points) const Vector3 u = x2 + mR2 - x1 - mR1; @@ -133,15 +132,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR1PlusUCrossN2 = (r1PlusU).cross(mN2); mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); + // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal sumInverseMass = body1MassInverse + body2MassInverse; - Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; - Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; + Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; + Vector3 I2R2CrossN1 = i2 * mR2CrossN1; + Vector3 I2R2CrossN2 = i2 * mR2CrossN2; const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -151,36 +153,40 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint.setToZero(); + Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + mWorld.mSliderJointsComponents.setInverseMassMatrixTranslation(mEntity, matrixKTranslation.getInverse()); } // Compute the bias "b" of the translation constraint - mBTranslation.setToZero(); + Vector2& biasTranslation = mWorld.mSliderJointsComponents.getBiasTranslation(mEntity); + biasTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBTranslation.x = u.dot(mN1); - mBTranslation.y = u.dot(mN2); - mBTranslation *= biasFactor; + biasTranslation.x = u.dot(mN1); + biasTranslation.y = u.dot(mN2); + biasTranslation *= biasFactor; + mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation); } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = mI1 + mI2; + mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse()); } // Compute the bias "b" of the rotation constraint - mBRotation.setToZero(); + Vector3& biasRotation = mWorld.mSliderJointsComponents.getBiasRotation(mEntity); + biasRotation.setToZero(); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse(); - mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); + const Quaternion qError = orientationBody2 * mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse(); + mWorld.mSliderJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV()); } // If the limits are enabled @@ -188,8 +194,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) mInverseMassMatrixLimit = sumInverseMass + - mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); + mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); @@ -219,8 +225,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (!constraintSolverData.isWarmStartingActive) { // Reset all the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); + Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity); + Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); + impulseTranslation.setToZero(); + impulseRotation.setToZero(); mImpulseLowerLimit = 0.0; mImpulseUpperLimit = 0.0; mImpulseMotor = 0.0; @@ -254,13 +262,16 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Compute the impulse P=J^T * lambda for the motor constraint of body 1 Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; + const Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity); + const Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); + // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - - mR1PlusUCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody1 = -mN1 * impulseTranslation.x - mN2 * impulseTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * impulseTranslation.x - + mR1PlusUCrossN2 * impulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 += -mImpulseRotation; + angularImpulseBody1 += -impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 linearImpulseBody1 += linearImpulseLimits; @@ -271,15 +282,15 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += mWorld.mSliderJointsComponents.getI1(mEntity) * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + - mR2CrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody2 = mN1 * impulseTranslation.x + mN2 * impulseTranslation.y; + Vector3 angularImpulseBody2 = mR2CrossN1 * impulseTranslation.x + + mR2CrossN2 * impulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 - angularImpulseBody2 += mImpulseRotation; + angularImpulseBody2 += impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 linearImpulseBody2 += -linearImpulseLimits; @@ -290,7 +301,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + w2 += mWorld.mSliderJointsComponents.getI2(mEntity) * angularImpulseBody2; } // Solve the velocity constraint @@ -309,6 +320,9 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; + const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); + // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); @@ -323,8 +337,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const Vector2 JvTranslation(el1, el2); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); - mImpulseTranslation += deltaLambda; + Vector2 deltaLambda = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity) * (-JvTranslation - mWorld.mSliderJointsComponents.getBiasTranslation(mEntity)); + mWorld.mSliderJointsComponents.setImpulseTranslation(mEntity, deltaLambda + mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity)); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; @@ -333,7 +347,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // 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 2 translation constraints of body 2 const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; @@ -341,7 +355,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; // --------------- Rotation Constraints --------------- // @@ -349,20 +363,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const Vector3 JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); - mImpulseRotation += deltaLambda2; + Vector3 deltaLambda2 = mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity) * + (-JvRotation - mWorld.mSliderJointsComponents.getBiasRotation(mEntity)); + mWorld.mSliderJointsComponents.setImpulseRotation(mEntity, deltaLambda2 + mWorld.mSliderJointsComponents.getImpulseRotation(mEntity)); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body to body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 = deltaLambda2; // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; // --------------- Limits Constraints --------------- // @@ -387,7 +402,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // 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 lower limit constraint of body 2 const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; @@ -395,7 +410,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } // If the upper limit is violated @@ -417,7 +432,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // 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 upper limit constraint of body 2 const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; @@ -425,7 +440,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } } @@ -483,12 +498,12 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inertia tensor of bodies - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Vector from body center to the anchor point - mR1 = q1 * mLocalAnchorPointBody1; - mR2 = q2 * mLocalAnchorPointBody2; + mR1 = q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); + mR2 = q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); // Compute the vector u (difference between anchor points) const Vector3 u = x2 + mR2 - x1 - mR1; @@ -517,15 +532,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // --------------- Translation Constraints --------------- // + const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); + // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); decimal sumInverseMass = body1MassInverse + body2MassInverse; - Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; - Vector3 I2R2CrossN2 = mI2 * mR2CrossN2; + Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; + Vector3 I2R2CrossN1 = i2 * mR2CrossN1; + Vector3 I2R2CrossN2 = i2 * mR2CrossN2; const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -535,18 +553,19 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint.setToZero(); + Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + mWorld.mSliderJointsComponents.setInverseMassMatrixTranslation(mEntity, matrixKTranslation.getInverse()); } // Compute the position error for the 2 translation constraints const Vector2 translationError(u.dot(mN1), u.dot(mN2)); // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 lambdaTranslation = mInverseMassMatrixTranslationConstraint * (-translationError); + Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; @@ -555,7 +574,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the 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; @@ -569,7 +588,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - Vector3 w2 = mI2 * angularImpulseBody2; + Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -580,11 +599,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = mI1 + mI2; + mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse()); } // Calculate difference in rotation @@ -602,7 +621,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // 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.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity) * q1.getInverse(); // A quaternion can be seen as: // @@ -616,13 +635,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = mInverseMassMatrixRotationConstraint * (-errorRotation); + Vector3 lambdaRotation = mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity) * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 = -lambdaRotation; // Apply the impulse to the body 1 - w1 = mI1 * angularImpulseBody1; + w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -632,7 +651,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint angularImpulseBody2 = lambdaRotation; // Apply the impulse to the body 2 - w2 = mI2 * angularImpulseBody2; + w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -648,8 +667,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + - mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); + mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); } @@ -666,7 +685,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + const Vector3 w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -679,7 +698,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mI2 * angularImpulseBody2; + const Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -699,7 +718,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mI1 * angularImpulseBody1; + const Vector3 w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -712,7 +731,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mI2 * angularImpulseBody2; + const Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -779,8 +798,8 @@ decimal SliderJoint::getTranslation() const { const Quaternion& q2 = transform2.getOrientation(); // Compute the two anchor points in world-space coordinates - const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; - const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2; + const Vector3 anchorBody1 = x1 + q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); + const Vector3 anchorBody2 = x2 + q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); // Compute the vector u (difference between anchor points) const Vector3 u = anchorBody2 - anchorBody1; @@ -868,3 +887,14 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { awakeBodies(); } } + +// Return a string representation +std::string SliderJoint::to_string() const { + return "SliderJoint{ lowerLimit=" + std::to_string(mLowerLimit) + ", upperLimit=" + std::to_string(mUpperLimit) + + "localAnchorPointBody1=" + mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + ", localAnchorPointBody2=" + + mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", sliderAxisBody1=" + mSliderAxisBody1.to_string() + + ", initOrientationDifferenceInv=" + + mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) + + ", maxMotorForce=" + std::to_string(mMaxMotorForce) + ", isLimitEnabled=" + + (mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}"; +} diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 501dc4d9..fc5b2e98 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -150,36 +150,21 @@ class SliderJoint : public Joint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; + /// Vector r1 in world-space coordinates + Vector3 mR1; - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; + /// Vector r2 in world-space coordinates + Vector3 mR2; /// Slider axis (in local-space coordinates of body 1) Vector3 mSliderAxisBody1; - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; - - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; - - /// Inverse of the initial orientation difference between the two bodies - Quaternion mInitOrientationDifferenceInv; - /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 Vector3 mN2; - /// Vector r1 in world-space coordinates - Vector3 mR1; - - /// Vector r2 in world-space coordinates - Vector3 mR2; - /// Cross product of r2 and n1 Vector3 mR2CrossN1; @@ -198,36 +183,18 @@ class SliderJoint : public Joint { /// Cross product of vector (r1 + u) and the slider axis Vector3 mR1PlusUCrossSliderAxis; - /// Bias of the 2 translation constraints - Vector2 mBTranslation; - - /// Bias of the 3 rotation constraints - Vector3 mBRotation; - /// Bias of the lower limit constraint decimal mBLowerLimit; /// Bias of the upper limit constraint decimal mBUpperLimit; - /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) - Matrix2x2 mInverseMassMatrixTranslationConstraint; - - /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) - Matrix3x3 mInverseMassMatrixRotationConstraint; - /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) decimal mInverseMassMatrixLimit; /// Inverse of mass matrix K=JM^-1J^t for the motor decimal mInverseMassMatrixMotor; - /// Accumulated impulse for the 2 translation constraints - Vector2 mImpulseTranslation; - - /// Accumulated impulse for the 3 rotation constraints - Vector3 mImpulseRotation; - /// Accumulated impulse for the lower limit constraint decimal mImpulseLowerLimit; @@ -408,17 +375,6 @@ inline size_t SliderJoint::getSizeInBytes() const { return sizeof(SliderJoint); } -// Return a string representation -inline std::string SliderJoint::to_string() const { - return "SliderJoint{ lowerLimit=" + std::to_string(mLowerLimit) + ", upperLimit=" + std::to_string(mUpperLimit) + - "localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() + ", localAnchorPointBody2=" + - mLocalAnchorPointBody2.to_string() + ", sliderAxisBody1=" + mSliderAxisBody1.to_string() + - ", initOrientationDifferenceInv=" + - mInitOrientationDifferenceInv.to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) + - ", maxMotorForce=" + std::to_string(mMaxMotorForce) + ", isLimitEnabled=" + - (mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}"; -} - } #endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 4d77fee6..b78eeb6a 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -42,6 +42,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()), mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mHingeJointsComponents(mMemoryManager.getBaseAllocator()), + mSliderJointsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), @@ -271,6 +272,9 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) { if (mHingeJointsComponents.hasComponent(jointEntity)) { mHingeJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); } + if (mSliderJointsComponents.hasComponent(jointEntity)) { + mSliderJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); + } } // Return true if two bodies overlap diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 95b5a5e3..7c071d4d 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -41,6 +41,7 @@ #include "components/BallAndSocketJointComponents.h" #include "components/FixedJointComponents.h" #include "components/HingeJointComponents.h" +#include "components/SliderJointComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -104,6 +105,9 @@ class CollisionWorld { /// Hinge joints Components HingeJointComponents mHingeJointsComponents; + /// Slider joints Components + SliderJointComponents mSliderJointsComponents; + /// Reference to the collision detection CollisionDetectionSystem mCollisionDetection; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 51ba7784..64a495ca 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -348,10 +348,18 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { + // Create a SliderJoint component + SliderJointComponents::SliderJointComponent sliderJointComponent; + mSliderJointsComponents.addComponent(entity, isJointDisabled, sliderJointComponent); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) SliderJoint(entity, *this, info); + SliderJoint* joint = new (allocatedMemory) SliderJoint(entity, *this, info); + + newJoint = joint; + mSliderJointsComponents.setJoint(entity, joint); + break; } @@ -461,10 +469,25 @@ void DynamicsWorld::destroyJoint(Joint* joint) { size_t nbBytes = joint->getSizeInBytes(); + Entity jointEntity = joint->getEntity(); + // Destroy the corresponding entity and its components // TODO : Make sure we remove all its components here - mJointsComponents.removeComponent(joint->getEntity()); - mEntityManager.destroyEntity(joint->getEntity()); + mJointsComponents.removeComponent(jointEntity); + mEntityManager.destroyEntity(jointEntity); + + if (mBallAndSocketJointsComponents.hasComponent(jointEntity)) { + mBallAndSocketJointsComponents.removeComponent(jointEntity); + } + if (mFixedJointsComponents.hasComponent(jointEntity)) { + mFixedJointsComponents.removeComponent(jointEntity); + } + if (mHingeJointsComponents.hasComponent(jointEntity)) { + mHingeJointsComponents.removeComponent(jointEntity); + } + if (mSliderJointsComponents.hasComponent(jointEntity)) { + mSliderJointsComponents.removeComponent(jointEntity); + } // Call the destructor of the joint joint->~Joint(); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 839e5ffe..5687c943 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -410,7 +410,6 @@ inline decimal DynamicsWorld::getTimeBeforeSleep() const { return mTimeBeforeSleep; } - // Set the time a body is required to stay still before sleeping /** * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)