From 06132e3d4139f1789f34ca0b95de7ec928ea58ea Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 11 Sep 2019 21:13:45 +0200 Subject: [PATCH] Add HingeJointComponents class --- CMakeLists.txt | 2 + src/components/HingeJointComponents.cpp | 408 +++++++++++ src/components/HingeJointComponents.h | 898 ++++++++++++++++++++++++ src/constraint/HingeJoint.cpp | 590 ++++++++++------ src/constraint/HingeJoint.h | 170 ----- src/engine/CollisionWorld.cpp | 5 +- src/engine/CollisionWorld.h | 4 + src/engine/DynamicsWorld.cpp | 14 +- 8 files changed, 1698 insertions(+), 393 deletions(-) create mode 100644 src/components/HingeJointComponents.cpp create mode 100644 src/components/HingeJointComponents.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d90813e..9e771c0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -149,6 +149,7 @@ SET (REACTPHYSICS3D_HEADERS "src/components/JointComponents.h" "src/components/BallAndSocketJointComponents.h" "src/components/FixedJointComponents.h" + "src/components/HingeJointComponents.h" "src/collision/CollisionCallback.h" "src/collision/OverlapCallback.h" "src/mathematics/mathematics.h" @@ -244,6 +245,7 @@ SET (REACTPHYSICS3D_SOURCES "src/components/JointComponents.cpp" "src/components/BallAndSocketJointComponents.cpp" "src/components/FixedJointComponents.cpp" + "src/components/HingeJointComponents.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 new file mode 100644 index 00000000..b3880e63 --- /dev/null +++ b/src/components/HingeJointComponents.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 "HingeJointComponents.h" +#include "engine/EntityManager.h" +#include "mathematics/Matrix3x3.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Constructor +HingeJointComponents::HingeJointComponents(MemoryAllocator& allocator) + :Components(allocator, sizeof(Entity) + sizeof(HingeJoint*) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + + sizeof(Vector2) + sizeof(Matrix3x3) + sizeof(Matrix2x2) + + sizeof(Vector3) + sizeof(Vector2) + 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 HingeJointComponents::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); + HingeJoint** 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); + Vector2* newImpulseRotation = reinterpret_cast(newImpulseTranslation + nbComponentsToAllocate); + Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast(newImpulseRotation + nbComponentsToAllocate); + Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast(newInverseMassMatrixTranslation + nbComponentsToAllocate); + Vector3* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); + Vector2* 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(HingeJoint*)); + 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(Vector2)); + memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix3x3)); + memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix2x2)); + memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector3)); + memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector2)); + 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; + mR1World = newR1World; + mR2World = newR2World; + 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 HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& 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); + 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 HingeJointComponents::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(mImpulseTranslation[srcIndex]); + new (mImpulseRotation + destIndex) Vector2(mImpulseRotation[srcIndex]); + new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]); + new (mInverseMassMatrixRotation + destIndex) Matrix2x2(mInverseMassMatrixRotation[srcIndex]); + new (mBiasTranslation + destIndex) Vector3(mBiasTranslation[srcIndex]); + new (mBiasRotation + destIndex) Vector2(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 HingeJointComponents::swapComponents(uint32 index1, uint32 index2) { + + // Copy component 1 data + Entity jointEntity1(mJointEntities[index1]); + HingeJoint* 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]); + Vector2 impulseRotation1(mImpulseRotation[index1]); + Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]); + Matrix2x2 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]); + Vector3 biasTranslation1(mBiasTranslation[index1]); + Vector2 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 (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) Vector2(impulseRotation1); + new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1); + new (mInverseMassMatrixRotation + index2) Matrix2x2(inverseMassMatrixRotation1); + new (mBiasTranslation + index2) Vector3(biasTranslation1); + new (mBiasRotation + index2) Vector2(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 HingeJointComponents::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].~Vector2(); + mInverseMassMatrixTranslation[index].~Matrix3x3(); + mInverseMassMatrixRotation[index].~Matrix2x2(); + mBiasTranslation[index].~Vector3(); + mBiasRotation[index].~Vector2(); + mInitOrientationDifferenceInv[index].~Quaternion(); + mHingeLocalAxisBody1[index].~Vector3(); + mHingeLocalAxisBody2[index].~Vector3(); + mA1[index].~Vector3(); + mB2CrossA1[index].~Vector3(); + mC2CrossA1[index].~Vector3(); +} diff --git a/src/components/HingeJointComponents.h b/src/components/HingeJointComponents.h new file mode 100644 index 00000000..20c5acd3 --- /dev/null +++ b/src/components/HingeJointComponents.h @@ -0,0 +1,898 @@ +/******************************************************************************** +* 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_HINGE_JOINT_COMPONENTS_H +#define REACTPHYSICS3D_HINGE_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 HingeJoint; +enum class JointType; + +// Class HingeJointComponents +/** + * This class represent the component of the ECS with data for the HingeJoint. + */ +class HingeJointComponents : public Components { + + private: + + // -------------------- Attributes -------------------- // + + /// Array of joint entities + Entity* mJointEntities; + + /// Array of pointers to the joints + HingeJoint** 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 + Vector2* 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) + Matrix2x2* mInverseMassMatrixRotation; + + /// Bias vector for the 3 translation constraints + Vector3* mBiasTranslation; + + /// Bias vector for the 3 rotation constraints + Vector2* 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 HingeJointComponent { + + bool isLimitEnabled; + bool isMotorEnabled; + decimal lowerLimit; + decimal upperLimit; + decimal motorSpeed; + decimal maxMotorTorque; + + /// Constructor + HingeJointComponent(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 + HingeJointComponents(MemoryAllocator& allocator); + + /// Destructor + virtual ~HingeJointComponents() override = default; + + /// Add a component + void addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& component); + + /// Return a pointer to a given joint + HingeJoint* getJoint(Entity jointEntity) const; + + /// Set the joint pointer to a given joint + void setJoint(Entity jointEntity, HingeJoint* 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 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 + Vector2& getImpulseRotation(Entity jointEntity); + + /// Set the translation impulse + void setImpulseRotation(Entity jointEntity, const Vector2& 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 + Matrix2x2& getInverseMassMatrixRotation(Entity jointEntity); + + /// Set the rotation inverse mass matrix of the constraint + void setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix); + + /// Return the translation bias + Vector3& getBiasTranslation(Entity jointEntity); + + /// Set the translation impulse + void setBiasTranslation(Entity jointEntity, const Vector3& impulseTranslation); + + /// Return the rotation bias + Vector2& getBiasRotation(Entity jointEntity); + + /// Set the rotation impulse + void setBiasRotation(Entity jointEntity, const Vector2& 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 HingeJoint* HingeJointComponents::getJoint(Entity jointEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mJoints[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the joint pointer to a given joint +inline void HingeJointComponents::setJoint(Entity jointEntity, HingeJoint* 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& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::setLocalAnchorPointBody2(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& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mI2[mMapEntityToComponentIndex[jointEntity]] = i2; +} + +// Return the translation impulse +inline Vector3& HingeJointComponents::getImpulseTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void HingeJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation impulse +inline Vector2& HingeJointComponents::getImpulseRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void HingeJointComponents::setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the translation inverse mass matrix of the constraint +inline Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + + +// Set the translation inverse mass matrix of the constraint +inline void HingeJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the rotation inverse mass matrix of the constraint +inline Matrix2x2& HingeJointComponents::getInverseMassMatrixRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation inverse mass matrix of the constraint +inline void HingeJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix; +} + +// Return the translation bias +inline Vector3& HingeJointComponents::getBiasTranslation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the translation impulse +inline void HingeJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation; +} + +// Return the rotation bias +inline Vector2 &HingeJointComponents::getBiasRotation(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mBiasRotation[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void HingeJointComponents::setBiasRotation(Entity jointEntity, const Vector2& impulseRotation) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation; +} + +// Return the initial orientation difference +inline Quaternion& HingeJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) { + + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]]; +} + +// Set the rotation impulse +inline void HingeJointComponents::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/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 12f1c6bb..113b68a4 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -35,34 +35,33 @@ using namespace reactphysics3d; const decimal HingeJoint::BETA = decimal(0.2); // Constructor -HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) - : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), - mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), - mIsLowerLimitViolated(false), mIsUpperLimitViolated(false), - mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) { +HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world, jointInfo) { - assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0) * PI); - assert(mUpperLimit >= decimal(0) && mUpperLimit <= decimal(2.0) * PI); + const decimal lowerLimit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity); + const decimal upperLimit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity); + assert(lowerLimit <= decimal(0) && lowerLimit >= decimal(-2.0) * PI); + assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0) * PI); // Compute the local-space anchor point for each body Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); // Compute the local-space hinge axis - mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; - mHingeLocalAxisBody1.normalize(); - mHingeLocalAxisBody2.normalize(); + Vector3 hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + Vector3 hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld; + hingeLocalAxisBody1.normalize(); + hingeLocalAxisBody2.normalize(); + mWorld.mHingeJointsComponents.setHingeLocalAxisBody1(mEntity, hingeLocalAxisBody1); + mWorld.mHingeJointsComponents.setHingeLocalAxisBody2(mEntity, hingeLocalAxisBody2); // Compute the inverse of the initial orientation difference between the two bodies - mInitOrientationDifferenceInv = transform2.getOrientation() * + Quaternion initOrientationDifferenceInv = transform2.getOrientation() * transform1.getOrientation().getInverse(); - mInitOrientationDifferenceInv.normalize(); - mInitOrientationDifferenceInv.inverse(); + initOrientationDifferenceInv.normalize(); + initOrientationDifferenceInv.inverse(); + mWorld.mHingeJointsComponents.setInitOrientationDifferenceInv(mEntity, initOrientationDifferenceInv); } // Initialize before solving the constraint @@ -83,43 +82,46 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); // Get the inertia tensor of bodies - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Compute the vector from body center to the anchor point in world-space - mR1World = orientationBody1 * mLocalAnchorPointBody1; - mR2World = orientationBody2 * mLocalAnchorPointBody2; + mWorld.mHingeJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity)); + mWorld.mHingeJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity)); // Compute the current angle around the hinge axis decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); // Check if the limit constraints are violated or not - decimal lowerLimitError = hingeAngle - mLowerLimit; - decimal upperLimitError = mUpperLimit - hingeAngle; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { - mImpulseLowerLimit = 0.0; + decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity); + decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle; + bool oldIsLowerLimitViolated = mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity); + bool isLowerLimitViolated = lowerLimitError <= 0; + mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated); + if (isLowerLimitViolated != oldIsLowerLimitViolated) { + mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { - mImpulseUpperLimit = 0.0; + bool oldIsUpperLimitViolated = mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity); + bool isUpperLimitViolated = upperLimitError <= 0; + mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated); + if (isUpperLimitViolated != oldIsUpperLimitViolated) { + mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0)); } // Compute vectors needed in the Jacobian - mA1 = orientationBody1 * mHingeLocalAxisBody1; - Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2; - mA1.normalize(); + Vector3 a1 = orientationBody1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity); + Vector3 a2 = orientationBody2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity); + a1.normalize(); a2.normalize(); + mWorld.mHingeJointsComponents.setA1(mEntity, a1); const Vector3 b2 = a2.getOneUnitOrthogonalVector(); const Vector3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2.cross(a1)); + mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2.cross(a1)); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR1World(mEntity)); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR2World(mEntity)); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); @@ -128,78 +130,93 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); - mInverseMassMatrixTranslation.setToZero(); + skewSymmetricMatrixU1 * mWorld.mHingeJointsComponents.getI1(mEntity) * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mWorld.mHingeJointsComponents.getI2(mEntity) * skewSymmetricMatrixU2.getTranspose(); + Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); + mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse()); } // Compute the bias "b" of the translation constraints - mBTranslation.setToZero(); + Vector3& bTranslation = mWorld.mHingeJointsComponents.getBiasTranslation(mEntity); + bTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World); + bTranslation = biasFactor * (x2 + mWorld.mHingeJointsComponents.getR2World(mEntity) - x1 - mWorld.mHingeJointsComponents.getR1World(mEntity)); + mWorld.mHingeJointsComponents.setBiasTranslation(mEntity, bTranslation); } + const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity); + const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity); + const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity); + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; - Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; - Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; - Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; - const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); + Vector3 i1B2CrossA1 = i1 * b2CrossA1; + Vector3 i1C2CrossA1 = i1 * c2CrossA1; + Vector3 i2B2CrossA1 = i2 * b2CrossA1; + Vector3 i2C2CrossA1 = i2 * c2CrossA1; + const decimal el11 = b2CrossA1.dot(i1B2CrossA1) + + b2CrossA1.dot(i2B2CrossA1); + const decimal el12 = b2CrossA1.dot(i1C2CrossA1) + + b2CrossA1.dot(i2C2CrossA1); + const decimal el21 = c2CrossA1.dot(i1B2CrossA1) + + c2CrossA1.dot(i2B2CrossA1); + const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + + c2CrossA1.dot(i2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - mInverseMassMatrixRotation.setToZero(); + Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity); + inverseMassMatrixRotation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotation = matrixKRotation.getInverse(); + mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse()); } // Compute the bias "b" of the rotation constraints - mBRotation.setToZero(); + Vector2& biasRotation = mWorld.mHingeJointsComponents.getBiasRotation(mEntity); + biasRotation.setToZero(); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2)); + mWorld.mHingeJointsComponents.setBiasRotation(mEntity, biasFactor * Vector2(a1.dot(b2), a1.dot(c2))); } // If warm-starting is not enabled if (!constraintSolverData.isWarmStartingActive) { // Reset all the accumulated impulses - mImpulseTranslation.setToZero(); - mImpulseRotation.setToZero(); - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; - mImpulseMotor = 0.0; + Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity); + Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity); + impulseTranslation.setToZero(); + impulseRotation.setToZero(); + mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); + mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0)); + mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0)); } // If the motor or limits are enabled - if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) { + if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) || + (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) || + mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)))) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) - mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); - mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); + decimal inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1); + inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0); + mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor); - if (mIsLimitEnabled) { + if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) { // Compute the bias "b" of the lower limit constraint - mBLowerLimit = 0.0; + mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, decimal(0.0)); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBLowerLimit = biasFactor * lowerLimitError; + mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError); } // Compute the bias "b" of the upper limit constraint - mBUpperLimit = 0.0; + mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, decimal(0.0)); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mBUpperLimit = biasFactor * upperLimitError; + mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError); } } } @@ -225,18 +242,27 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity); + const Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity); + + const decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity); + const decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity); + + const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity); + const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity); + // Compute the impulse P=J^T * lambda for the 2 rotation constraints - Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; + Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mWorld.mHingeJointsComponents.getC2CrossA1(mEntity) * impulseRotation.y; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints - const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1; + const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1; // Compute the impulse P=J^T * lambda for the motor constraint - const Vector3 motorImpulse = -mImpulseMotor * mA1; + const Vector3 motorImpulse = -mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) * a1; // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mImpulseTranslation; - Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World); + Vector3 linearImpulseBody1 = -impulseTranslation; + Vector3 angularImpulseBody1 = impulseTranslation.cross(mWorld.mHingeJointsComponents.getR1World(mEntity)); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 angularImpulseBody1 += rotationImpulse; @@ -249,10 +275,10 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mI1 * angularImpulseBody1; + w1 += mWorld.mHingeJointsComponents.getI1(mEntity) * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 - Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World); + Vector3 angularImpulseBody2 = -impulseTranslation.cross(mWorld.mHingeJointsComponents.getR2World(mEntity)); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 angularImpulseBody2 += -rotationImpulse; @@ -264,8 +290,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { angularImpulseBody2 += -motorImpulse; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * mImpulseTranslation; - w2 += mI2 * angularImpulseBody2; + v2 += inverseMassBody2 * impulseTranslation; + w2 += mWorld.mHingeJointsComponents.getI2(mEntity) * angularImpulseBody2; } // Solve the velocity constraint @@ -288,136 +314,158 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity); + + const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity); + + const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity); + const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity); + + const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity); + + const decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity); + // --------------- Translation Constraints --------------- // // Compute J*v - const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); + const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); // Compute the Lagrange multiplier lambda - const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation * - (-JvTranslation - mBTranslation); - mImpulseTranslation += deltaLambdaTranslation; + const Vector3 deltaLambdaTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity) * + (-JvTranslation - mWorld.mHingeJointsComponents.getBiasTranslation(mEntity)); + mWorld.mHingeJointsComponents.setImpulseTranslation(mEntity, deltaLambdaTranslation + mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity)); // Compute the impulse P=J^T * lambda of body 1 const Vector3 linearImpulseBody1 = -deltaLambdaTranslation; - Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World); + Vector3 angularImpulseBody1 = deltaLambdaTranslation.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 of body 2 - Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World); + Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambdaTranslation; - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; // --------------- Rotation Constraints --------------- // // Compute J*v for the 2 rotation constraints - const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2), - -mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2)); + const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2), + -c2CrossA1.dot(w1) + c2CrossA1.dot(w2)); // Compute the Lagrange multiplier lambda for the 2 rotation constraints - Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation); - mImpulseRotation += deltaLambdaRotation; + Vector2 deltaLambdaRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity) * + (-JvRotation - mWorld.mHingeJointsComponents.getBiasRotation(mEntity)); + mWorld.mHingeJointsComponents.setImpulseRotation(mEntity, deltaLambdaRotation + mWorld.mHingeJointsComponents.getImpulseRotation(mEntity)); // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x - - mC2CrossA1 * deltaLambdaRotation.y; + angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - + c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 - angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x + - mC2CrossA1 * deltaLambdaRotation.y; + angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) { // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) { + + decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity); // Compute J*v for the lower limit constraint - const decimal JvLowerLimit = (w2 - w1).dot(mA1); + const decimal JvLowerLimit = (w2 - w1).dot(a1); // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit); - decimal lambdaTemp = mImpulseLowerLimit; - mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); - deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mWorld.mHingeJointsComponents.getBLowerLimit(mEntity)); + decimal lambdaTemp = impulseLowerLimit; + impulseLowerLimit = std::max(impulseLowerLimit + deltaLambdaLower, decimal(0.0)); + deltaLambdaLower = impulseLowerLimit - lambdaTemp; + mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, impulseLowerLimit); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1; + const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1; // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1; + const Vector3 angularImpulseBody2 = deltaLambdaLower * a1; // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) { + + decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity); // Compute J*v for the upper limit constraint - const decimal JvUpperLimit = -(w2 - w1).dot(mA1); + const decimal JvUpperLimit = -(w2 - w1).dot(a1); // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit); - decimal lambdaTemp = mImpulseUpperLimit; - mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); - deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mWorld.mHingeJointsComponents.getBUpperLimit(mEntity)); + decimal lambdaTemp = impulseUpperLimit; + impulseUpperLimit = std::max(impulseUpperLimit + deltaLambdaUpper, decimal(0.0)); + deltaLambdaUpper = impulseUpperLimit - lambdaTemp; + mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, impulseUpperLimit); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1; + const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1; // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1; + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1; // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } } // --------------- Motor --------------- // // If the motor is enabled - if (mIsMotorEnabled) { + if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity)) { + + decimal impulseMotor = mWorld.mHingeJointsComponents.getImpulseMotor(mEntity); // Compute J*v for the motor - const decimal JvMotor = mA1.dot(w1 - w2); + const decimal JvMotor = a1.dot(w1 - w2); // Compute the Lagrange multiplier lambda for the motor - const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep; - decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed); - decimal lambdaTemp = mImpulseMotor; - mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); - deltaLambdaMotor = mImpulseMotor - lambdaTemp; + const decimal maxMotorImpulse = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity) * constraintSolverData.timeStep; + decimal deltaLambdaMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity) * (-JvMotor - mWorld.mHingeJointsComponents.getMotorSpeed(mEntity)); + decimal lambdaTemp = impulseMotor; + impulseMotor = clamp(impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = impulseMotor - lambdaTemp; + mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, impulseMotor); // Compute the impulse P=J^T * lambda for the motor of body 1 - const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1; // Apply the impulse to the body 1 - w1 += mI1 * angularImpulseBody1; + w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1; + const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1; // Apply the impulse to the body 2 - w2 += mI2 * angularImpulseBody2; + w2 += i2 * angularImpulseBody2; } } @@ -436,6 +484,17 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity); + const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity); + + const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity); + const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity); + + Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity); + Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity); + + Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity); + // Get the bodies positions and orientations Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity); @@ -447,35 +506,38 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inverse inertia tensors - mI1 = body1->getInertiaTensorInverseWorld(); - mI2 = body2->getInertiaTensorInverseWorld(); + mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); + mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Compute the vector from body center to the anchor point in world-space - mR1World = q1 * mLocalAnchorPointBody1; - mR2World = q2 * mLocalAnchorPointBody2; + mWorld.mHingeJointsComponents.setR1World(mEntity, q1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity)); + mWorld.mHingeJointsComponents.setR2World(mEntity, q2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity)); // Compute the current angle around the hinge axis decimal hingeAngle = computeCurrentHingeAngle(q1, q2); // Check if the limit constraints are violated or not - decimal lowerLimitError = hingeAngle - mLowerLimit; - decimal upperLimitError = mUpperLimit - hingeAngle; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity); + decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle; + mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0); + mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0); // Compute vectors needed in the Jacobian - mA1 = q1 * mHingeLocalAxisBody1; - Vector3 a2 = q2 * mHingeLocalAxisBody2; - mA1.normalize(); + a1 = q1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity); + Vector3 a2 = q2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity); + a1.normalize(); + mWorld.mHingeJointsComponents.setA1(mEntity, a1); a2.normalize(); const Vector3 b2 = a2.getOneUnitOrthogonalVector(); const Vector3 c2 = a2.cross(b2); - mB2CrossA1 = b2.cross(mA1); - mC2CrossA1 = c2.cross(mA1); + b2CrossA1 = b2.cross(a1); + mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2CrossA1); + c2CrossA1 = c2.cross(a1); + mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2CrossA1); // 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 --------------- // @@ -486,27 +548,29 @@ void HingeJoint::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.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity); + inverseMassMatrixTranslation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixTranslation = massMatrix.getInverse(); + inverseMassMatrixTranslation = massMatrix.getInverse(); + mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, inverseMassMatrixTranslation); } // 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; @@ -514,11 +578,11 @@ void HingeJoint::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; @@ -528,46 +592,47 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Rotation Constraints --------------- // // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix) - Vector3 I1B2CrossA1 = mI1 * mB2CrossA1; - Vector3 I1C2CrossA1 = mI1 * mC2CrossA1; - Vector3 I2B2CrossA1 = mI2 * mB2CrossA1; - Vector3 I2C2CrossA1 = mI2 * mC2CrossA1; - const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) + - mB2CrossA1.dot(I2B2CrossA1); - const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) + - mB2CrossA1.dot(I2C2CrossA1); - const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) + - mC2CrossA1.dot(I2B2CrossA1); - const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) + - mC2CrossA1.dot(I2C2CrossA1); + Vector3 I1B2CrossA1 = i1 * b2CrossA1; + Vector3 I1C2CrossA1 = i1 * c2CrossA1; + Vector3 I2B2CrossA1 = i2 * b2CrossA1; + Vector3 I2C2CrossA1 = i2 * c2CrossA1; + const decimal el11 = b2CrossA1.dot(I1B2CrossA1) + + b2CrossA1.dot(I2B2CrossA1); + const decimal el12 = b2CrossA1.dot(I1C2CrossA1) + + b2CrossA1.dot(I2C2CrossA1); + const decimal el21 = c2CrossA1.dot(I1B2CrossA1) + + c2CrossA1.dot(I2B2CrossA1); + const decimal el22 = c2CrossA1.dot(I1C2CrossA1) + + c2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); - mInverseMassMatrixRotation.setToZero(); + Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity); + inverseMassMatrixRotation.setToZero(); if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mInverseMassMatrixRotation = matrixKRotation.getInverse(); + mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse()); } // Compute the position error for the 3 rotation constraints - const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2)); + const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2)); // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation); + Vector2 lambdaRotation = inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y; + angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; // 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 impulse of body 2 - angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y; + angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 2 - w2 = mI2 * angularImpulseBody2; + w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -575,37 +640,40 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) { - if (mIsLowerLimitViolated || mIsUpperLimitViolated) { + decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity); + + if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1); - mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0); + inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1); + inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0); + mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor); } // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) { // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError ); + decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError ); // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1; + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mI1 * angularImpulseBody1; + const Vector3 w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1; + const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mI2 * angularImpulseBody2; + const Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -613,26 +681,26 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) { // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError); + decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError); // Compute the impulse P=J^T * lambda of body 1 - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1; + const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mI1 * angularImpulseBody1; + const Vector3 w1 = i1 * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1; + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mI2 * angularImpulseBody2; + const Vector3 w2 = i2 * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -654,9 +722,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS */ void HingeJoint::enableLimit(bool isLimitEnabled) { - if (isLimitEnabled != mIsLimitEnabled) { + if (isLimitEnabled != mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) { - mIsLimitEnabled = isLimitEnabled; + mWorld.mHingeJointsComponents.setIsLimitEnabled(mEntity, isLimitEnabled); // Reset the limits resetLimits(); @@ -670,8 +738,8 @@ void HingeJoint::enableLimit(bool isLimitEnabled) { */ void HingeJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; - mImpulseMotor = 0.0; + mWorld.mHingeJointsComponents.setIsMotorEnabled(mEntity, isMotorEnabled); + mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0)); // Wake up the two bodies of the joint awakeBodies(); @@ -683,11 +751,13 @@ void HingeJoint::enableMotor(bool isMotorEnabled) { */ void HingeJoint::setMinAngleLimit(decimal lowerLimit) { - assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0 * PI)); + const decimal limit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity); - if (lowerLimit != mLowerLimit) { + assert(limit <= decimal(0.0) && limit >= decimal(-2.0) * PI); - mLowerLimit = lowerLimit; + if (lowerLimit != limit) { + + mWorld.mHingeJointsComponents.setLowerLimit(mEntity, lowerLimit); // Reset the limits resetLimits(); @@ -700,11 +770,13 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) { */ void HingeJoint::setMaxAngleLimit(decimal upperLimit) { - assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0 * PI)); + const decimal limit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity); - if (upperLimit != mUpperLimit) { + assert(limit >= decimal(0) && limit <= decimal(2.0) * PI); - mUpperLimit = upperLimit; + if (upperLimit != limit) { + + mWorld.mHingeJointsComponents.setUpperLimit(mEntity, upperLimit); // Reset the limits resetLimits(); @@ -715,8 +787,8 @@ void HingeJoint::setMaxAngleLimit(decimal upperLimit) { void HingeJoint::resetLimits() { // Reset the accumulated impulses for the limits - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; + mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); + mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0)); // Wake up the two bodies of the joint awakeBodies(); @@ -725,9 +797,9 @@ void HingeJoint::resetLimits() { // Set the motor speed void HingeJoint::setMotorSpeed(decimal motorSpeed) { - if (motorSpeed != mMotorSpeed) { + if (motorSpeed != mWorld.mHingeJointsComponents.getMotorSpeed(mEntity)) { - mMotorSpeed = motorSpeed; + mWorld.mHingeJointsComponents.setMotorSpeed(mEntity, motorSpeed); // Wake up the two bodies of the joint awakeBodies(); @@ -740,10 +812,12 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) { */ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { - if (maxMotorTorque != mMaxMotorTorque) { + const decimal torque = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity); - assert(mMaxMotorTorque >= decimal(0.0)); - mMaxMotorTorque = maxMotorTorque; + if (maxMotorTorque != torque) { + + assert(torque >= decimal(0.0)); + mWorld.mHingeJointsComponents.setMaxMotorTorque(mEntity, maxMotorTorque); // Wake up the two bodies of the joint awakeBodies(); @@ -771,8 +845,7 @@ decimal HingeJoint::computeNormalizedAngle(decimal angle) const { // Given an "inputAngle" in the range [-pi, pi], this method returns an // angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the // two angle limits in arguments. -decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, - decimal upperLimitAngle) const { +decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const { if (upperLimitAngle <= lowerLimitAngle) { return inputAngle; } @@ -792,8 +865,7 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci } // Compute the current angle around the hinge axis -decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, - const Quaternion& orientationBody2) { +decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, const Quaternion& orientationBody2) { decimal hingeAngle; @@ -802,7 +874,7 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, currentOrientationDiff.normalize(); // Compute the relative rotation considering the initial orientation difference - Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv; + Quaternion relativeRotation = currentOrientationDiff * mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity); relativeRotation.normalize(); // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit @@ -816,7 +888,7 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, decimal sinHalfAngleAbs = relativeRotation.getVectorV().length(); // Compute the dot product of the relative rotation axis and the hinge axis - decimal dotProduct = relativeRotation.getVectorV().dot(mA1); + decimal dotProduct = relativeRotation.getVectorV().dot(mWorld.mHingeJointsComponents.getA1(mEntity)); // If the relative rotation axis and the hinge axis are pointing the same direction if (dotProduct >= decimal(0.0)) { @@ -830,6 +902,84 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, hingeAngle = computeNormalizedAngle(hingeAngle); // Compute and return the corresponding angle near one the two limits - return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit); + return computeCorrespondingAngleNearLimits(hingeAngle, + mWorld.mHingeJointsComponents.getLowerLimit(mEntity), + mWorld.mHingeJointsComponents.getUpperLimit(mEntity)); } +// Return true if the limits of the joint are enabled +/** + * @return True if the limits of the joint are enabled and false otherwise + */ +bool HingeJoint::isLimitEnabled() const { + return mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity); +} + +// Return true if the motor of the joint is enabled +/** + * @return True if the motor of joint is enabled and false otherwise + */ +bool HingeJoint::isMotorEnabled() const { + return mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity); +} + +// Return the minimum angle limit +/** + * @return The minimum limit angle of the joint (in radian) + */ +decimal HingeJoint::getMinAngleLimit() const { + return mWorld.mHingeJointsComponents.getLowerLimit(mEntity); +} + +// Return the maximum angle limit +/** + * @return The maximum limit angle of the joint (in radian) + */ +decimal HingeJoint::getMaxAngleLimit() const { + return mWorld.mHingeJointsComponents.getUpperLimit(mEntity); +} + +// Return the motor speed +/** + * @return The current speed of the joint motor (in radian per second) + */ + decimal HingeJoint::getMotorSpeed() const { + return mWorld.mHingeJointsComponents.getMotorSpeed(mEntity); +} + +// Return the maximum motor torque +/** + * @return The maximum torque of the joint motor (in Newtons) + */ + decimal HingeJoint::getMaxMotorTorque() const { + return mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity); +} + +// Return the intensity of the current torque applied for the joint motor +/** + * @param timeStep The current time step (in seconds) + * @return The intensity of the current torque (in Newtons) of the joint motor + */ + decimal HingeJoint::getMotorTorque(decimal timeStep) const { + return mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) / timeStep; +} + +// Return the number of bytes used by the joint + size_t HingeJoint::getSizeInBytes() const { + return sizeof(HingeJoint); +} + +// Return a string representation +std::string HingeJoint::to_string() const { + return "HingeJoint{ lowerLimit=" + std::to_string(mWorld.mHingeJointsComponents.getLowerLimit(mEntity)) + + ", upperLimit=" + std::to_string(mWorld.mHingeJointsComponents.getUpperLimit(mEntity)) + + "localAnchorPointBody1=" + mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + ", localAnchorPointBody2=" + + mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", hingeLocalAxisBody1=" + + mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity).to_string() + + ", hingeLocalAxisBody2=" + mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity).to_string() + + ", initOrientationDifferenceInv=" + mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + + ", motorSpeed=" + std::to_string(mWorld.mHingeJointsComponents.getMotorSpeed(mEntity)) + + ", maxMotorTorque=" + std::to_string(mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity)) + ", isLimitEnabled=" + + (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) ? "true" : "false") + ", isMotorEnabled=" + + (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) ? "true" : "false") + "}"; +} diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index 1186c043..e16d7a40 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -149,104 +149,6 @@ class HingeJoint : public Joint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local-space coordinates of body 1) - Vector3 mLocalAnchorPointBody1; - - /// Anchor point of body 2 (in local-space coordinates of body 2) - Vector3 mLocalAnchorPointBody2; - - /// Hinge rotation axis (in local-space coordinates of body 1) - Vector3 mHingeLocalAxisBody1; - - /// Hinge rotation axis (in local-space coordiantes of body 2) - Vector3 mHingeLocalAxisBody2; - - /// Inertia tensor of body 1 (in world-space coordinates) - Matrix3x3 mI1; - - /// Inertia tensor of body 2 (in world-space coordinates) - Matrix3x3 mI2; - - /// Hinge rotation axis (in world-space coordinates) computed from body 1 - Vector3 mA1; - - /// 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; - - /// Cross product of vector b2 and a1 - Vector3 mB2CrossA1; - - /// Cross product of vector c2 and a1; - Vector3 mC2CrossA1; - - /// Impulse for the 3 translation constraints - Vector3 mImpulseTranslation; - - /// Impulse for the 2 rotation constraints - Vector2 mImpulseRotation; - - /// 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 mass matrix K=JM^-1J^t for the 3 translation constraints - Matrix3x3 mInverseMassMatrixTranslation; - - /// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints - Matrix2x2 mInverseMassMatrixRotation; - - /// 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 vector for the error correction for the translation constraints - Vector3 mBTranslation; - - /// Bias vector for the error correction for the rotation constraints - Vector2 mBRotation; - - /// Bias of the lower limit constraint - decimal mBLowerLimit; - - /// Bias of the upper limit constraint - decimal mBUpperLimit; - - /// Inverse of the initial orientation difference between the bodies - Quaternion mInitOrientationDifferenceInv; - - /// 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 -------------------- // @@ -341,78 +243,6 @@ class HingeJoint : public Joint { virtual std::string to_string() const override; }; -// Return true if the limits of the joint are enabled -/** - * @return True if the limits of the joint are enabled and false otherwise - */ -inline bool HingeJoint::isLimitEnabled() const { - return mIsLimitEnabled; -} - -// Return true if the motor of the joint is enabled -/** - * @return True if the motor of joint is enabled and false otherwise - */ -inline bool HingeJoint::isMotorEnabled() const { - return mIsMotorEnabled; -} - -// Return the minimum angle limit -/** - * @return The minimum limit angle of the joint (in radian) - */ -inline decimal HingeJoint::getMinAngleLimit() const { - return mLowerLimit; -} - -// Return the maximum angle limit -/** - * @return The maximum limit angle of the joint (in radian) - */ -inline decimal HingeJoint::getMaxAngleLimit() const { - return mUpperLimit; -} - -// Return the motor speed -/** - * @return The current speed of the joint motor (in radian per second) - */ -inline decimal HingeJoint::getMotorSpeed() const { - return mMotorSpeed; -} - -// Return the maximum motor torque -/** - * @return The maximum torque of the joint motor (in Newtons) - */ -inline decimal HingeJoint::getMaxMotorTorque() const { - return mMaxMotorTorque; -} - -// Return the intensity of the current torque applied for the joint motor -/** - * @param timeStep The current time step (in seconds) - * @return The intensity of the current torque (in Newtons) of the joint motor - */ -inline decimal HingeJoint::getMotorTorque(decimal timeStep) const { - return mImpulseMotor / timeStep; -} - -// Return the number of bytes used by the joint -inline size_t HingeJoint::getSizeInBytes() const { - return sizeof(HingeJoint); -} - -// Return a string representation -inline std::string HingeJoint::to_string() const { - return "HingeJoint{ lowerLimit=" + std::to_string(mLowerLimit) + ", upperLimit=" + std::to_string(mUpperLimit) + - "localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() + ", localAnchorPointBody2=" + - mLocalAnchorPointBody2.to_string() + ", hingeLocalAxisBody1=" + mHingeLocalAxisBody1.to_string() + - ", hingeLocalAxisBody2=" + mHingeLocalAxisBody2.to_string() + ", initOrientationDifferenceInv=" + - mInitOrientationDifferenceInv.to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) + - ", maxMotorTorque=" + std::to_string(mMaxMotorTorque) + ", isLimitEnabled=" + - (mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}"; -} } diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index b12ed42f..4d77fee6 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -41,7 +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()), + mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mHingeJointsComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), @@ -268,6 +268,9 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) { if (mFixedJointsComponents.hasComponent(jointEntity)) { mFixedJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); } + if (mHingeJointsComponents.hasComponent(jointEntity)) { + mHingeJointsComponents.setIsEntityDisabled(jointEntity, isDisabled); + } } // Return true if two bodies overlap diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index ab7fc92f..95b5a5e3 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -40,6 +40,7 @@ #include "components/JointComponents.h" #include "components/BallAndSocketJointComponents.h" #include "components/FixedJointComponents.h" +#include "components/HingeJointComponents.h" #include "collision/CollisionCallback.h" #include "collision/OverlapCallback.h" @@ -100,6 +101,9 @@ class CollisionWorld { /// Fixed joints Components FixedJointComponents mFixedJointsComponents; + /// Hinge joints Components + HingeJointComponents mHingeJointsComponents; + /// Reference to the collision detection CollisionDetectionSystem mCollisionDetection; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index a784a82d..51ba7784 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -358,10 +358,20 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { + const HingeJointInfo& info = static_cast(jointInfo); + + // Create a HingeJoint component + HingeJointComponents::HingeJointComponent hingeJointComponent(info.isLimitEnabled, info.isMotorEnabled, + info.minAngleLimit, info.maxAngleLimit, + info.motorSpeed, info.maxMotorTorque); + mHingeJointsComponents.addComponent(entity, isJointDisabled, hingeJointComponent); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(HingeJoint)); - const HingeJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) HingeJoint(entity, *this, info); + HingeJoint* joint = new (allocatedMemory) HingeJoint(entity, *this, info); + + newJoint = joint; + mHingeJointsComponents.setJoint(entity, joint); break; }