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

Working on SliderJointComponents class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-09-19 17:12:52 +02:00
parent 0c0ff46d34
commit 170a1bfdfd
6 changed files with 703 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/components/HingeJointComponents.cpp
View File

@ -210,6 +210,7 @@ void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, con
new (mC2CrossA1 + index) Vector3(0, 0, 0); new (mC2CrossA1 + index) Vector3(0, 0, 0);
mImpulseLowerLimit[index] = decimal(0.0); mImpulseLowerLimit[index] = decimal(0.0);
mImpulseUpperLimit[index] = decimal(0.0); mImpulseUpperLimit[index] = decimal(0.0);
mImpulseMotor[index] = decimal(0.0);
mInverseMassMatrixLimitMotor[index] = decimal(0.0); mInverseMassMatrixLimitMotor[index] = decimal(0.0);
mInverseMassMatrixMotor[index] = decimal(0.0); mInverseMassMatrixMotor[index] = decimal(0.0);
mBLowerLimit[index] = decimal(0.0); mBLowerLimit[index] = decimal(0.0);

195
src/components/SliderJointComponents.cpp
View File

@ -35,15 +35,17 @@ using namespace reactphysics3d;
// Constructor // Constructor
SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator) SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(SliderJoint*) + sizeof(Vector3) + :Components(allocator, sizeof(Entity) + sizeof(SliderJoint*) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector2) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector2) +
sizeof(Vector3) + sizeof(Matrix2x2) + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Matrix2x2) + sizeof(Matrix3x3) +
sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion)/* + sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) +
sizeof(decimal) + 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) +
sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal)*/) { sizeof(bool) + sizeof(bool) + sizeof(decimal) + sizeof(decimal) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3)) {
// Allocate memory for the components data // Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS); allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -75,17 +77,17 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
Vector2* newBiasTranslation = reinterpret_cast<Vector2*>(newInverseMassMatrixRotation + nbComponentsToAllocate); Vector2* newBiasTranslation = reinterpret_cast<Vector2*>(newInverseMassMatrixRotation + nbComponentsToAllocate);
Vector3* newBiasRotation = reinterpret_cast<Vector3*>(newBiasTranslation + nbComponentsToAllocate); Vector3* newBiasRotation = reinterpret_cast<Vector3*>(newBiasTranslation + nbComponentsToAllocate);
Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(newBiasRotation + nbComponentsToAllocate); Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(newBiasRotation + nbComponentsToAllocate);
/* Vector3* newSliderAxisBody1 = reinterpret_cast<Vector3*>(newInitOrientationDifferenceInv + nbComponentsToAllocate);
Vector3* newHingeLocalAxisBody1 = reinterpret_cast<Vector3*>(newInitOrientationDifferenceInv + nbComponentsToAllocate); Vector3* newSliderAxisWorld = reinterpret_cast<Vector3*>(newSliderAxisBody1 + nbComponentsToAllocate);
Vector3* newHingeLocalAxisBody2 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody1 + nbComponentsToAllocate); Vector3* newR1 = reinterpret_cast<Vector3*>(newSliderAxisWorld + nbComponentsToAllocate);
Vector3* newA1 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody2 + nbComponentsToAllocate); Vector3* newR2 = reinterpret_cast<Vector3*>(newR1 + nbComponentsToAllocate);
Vector3* newB2CrossA1 = reinterpret_cast<Vector3*>(newA1 + nbComponentsToAllocate); Vector3* newN1 = reinterpret_cast<Vector3*>(newR2 + nbComponentsToAllocate);
Vector3* newC2CrossA1 = reinterpret_cast<Vector3*>(newB2CrossA1 + nbComponentsToAllocate); Vector3* newN2 = reinterpret_cast<Vector3*>(newN1 + nbComponentsToAllocate);
decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(newC2CrossA1 + nbComponentsToAllocate); decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(newN2 + nbComponentsToAllocate);
decimal* newImpulseUpperLimit = reinterpret_cast<decimal*>(newImpulseLowerLimit + nbComponentsToAllocate); decimal* newImpulseUpperLimit = reinterpret_cast<decimal*>(newImpulseLowerLimit + nbComponentsToAllocate);
decimal* newImpulseMotor = reinterpret_cast<decimal*>(newImpulseUpperLimit + nbComponentsToAllocate); decimal* newImpulseMotor = reinterpret_cast<decimal*>(newImpulseUpperLimit + nbComponentsToAllocate);
decimal* newInverseMassMatrixLimitMotor = reinterpret_cast<decimal*>(newImpulseMotor + nbComponentsToAllocate); decimal* newInverseMassMatrixLimit = reinterpret_cast<decimal*>(newImpulseMotor + nbComponentsToAllocate);
decimal* newInverseMassMatrixMotor = reinterpret_cast<decimal*>(newInverseMassMatrixLimitMotor + nbComponentsToAllocate); decimal* newInverseMassMatrixMotor = reinterpret_cast<decimal*>(newInverseMassMatrixLimit + nbComponentsToAllocate);
decimal* newBLowerLimit = reinterpret_cast<decimal*>(newInverseMassMatrixMotor + nbComponentsToAllocate); decimal* newBLowerLimit = reinterpret_cast<decimal*>(newInverseMassMatrixMotor + nbComponentsToAllocate);
decimal* newBUpperLimit = reinterpret_cast<decimal*>(newBLowerLimit + nbComponentsToAllocate); decimal* newBUpperLimit = reinterpret_cast<decimal*>(newBLowerLimit + nbComponentsToAllocate);
bool* newIsLimitEnabled = reinterpret_cast<bool*>(newBUpperLimit + nbComponentsToAllocate); bool* newIsLimitEnabled = reinterpret_cast<bool*>(newBUpperLimit + nbComponentsToAllocate);
@ -95,8 +97,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
bool* newIsLowerLimitViolated = reinterpret_cast<bool*>(newUpperLimit + nbComponentsToAllocate); bool* newIsLowerLimitViolated = reinterpret_cast<bool*>(newUpperLimit + nbComponentsToAllocate);
bool* newIsUpperLimitViolated = reinterpret_cast<bool*>(newIsLowerLimitViolated + nbComponentsToAllocate); bool* newIsUpperLimitViolated = reinterpret_cast<bool*>(newIsLowerLimitViolated + nbComponentsToAllocate);
decimal* newMotorSpeed = reinterpret_cast<decimal*>(newIsUpperLimitViolated + nbComponentsToAllocate); decimal* newMotorSpeed = reinterpret_cast<decimal*>(newIsUpperLimitViolated + nbComponentsToAllocate);
decimal* newMaxMotorTorque = reinterpret_cast<decimal*>(newMotorSpeed + nbComponentsToAllocate); decimal* newMaxMotorForce = reinterpret_cast<decimal*>(newMotorSpeed + nbComponentsToAllocate);
*/ Vector3* newR2CrossN1 = reinterpret_cast<Vector3*>(newMaxMotorForce + nbComponentsToAllocate);
Vector3* newR2CrossN2 = reinterpret_cast<Vector3*>(newR2CrossN1 + nbComponentsToAllocate);
Vector3* newR2CrossSliderAxis = reinterpret_cast<Vector3*>(newR2CrossN2 + nbComponentsToAllocate);
Vector3* newR1PlusUCrossN1 = reinterpret_cast<Vector3*>(newR2CrossSliderAxis + nbComponentsToAllocate);
Vector3* newR1PlusUCrossN2 = reinterpret_cast<Vector3*>(newR1PlusUCrossN1 + nbComponentsToAllocate);
Vector3* newR1PlusUCrossSliderAxis = reinterpret_cast<Vector3*>(newR1PlusUCrossN2 + nbComponentsToAllocate);
// If there was already components before // If there was already components before
if (mNbComponents > 0) { if (mNbComponents > 0) {
@ -115,16 +122,16 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector2)); memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector2));
memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3)); memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3));
memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion)); memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion));
/* memcpy(newSliderAxisBody1, mSliderAxisBody1, mNbComponents * sizeof(Vector3));
memcpy(newHingeLocalAxisBody1, mHingeLocalAxisBody1, mNbComponents * sizeof(Vector3)); memcpy(newSliderAxisWorld, mSliderAxisWorld, mNbComponents * sizeof(Vector3));
memcpy(newHingeLocalAxisBody2, mHingeLocalAxisBody2, mNbComponents * sizeof(Vector3)); memcpy(newR1, mR1, mNbComponents * sizeof(Vector3));
memcpy(newA1, mA1, mNbComponents * sizeof(Vector3)); memcpy(newR2, mR2, mNbComponents * sizeof(Vector3));
memcpy(newB2CrossA1, mB2CrossA1, mNbComponents * sizeof(Vector3)); memcpy(newN1, mN1, mNbComponents * sizeof(Vector3));
memcpy(newC2CrossA1, mC2CrossA1, mNbComponents * sizeof(Vector3)); memcpy(newN2, mN2, mNbComponents * sizeof(Vector3));
memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal));
memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal));
memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal)); memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal));
memcpy(newInverseMassMatrixLimitMotor, mInverseMassMatrixLimitMotor, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixLimit, mInverseMassMatrixLimit, mNbComponents * sizeof(decimal));
memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal));
memcpy(newBLowerLimit, mBLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newBLowerLimit, mBLowerLimit, mNbComponents * sizeof(decimal));
memcpy(newBUpperLimit, mBUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newBUpperLimit, mBUpperLimit, mNbComponents * sizeof(decimal));
@ -135,8 +142,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newIsLowerLimitViolated, mIsLowerLimitViolated, mNbComponents * sizeof(bool)); memcpy(newIsLowerLimitViolated, mIsLowerLimitViolated, mNbComponents * sizeof(bool));
memcpy(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool)); memcpy(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool));
memcpy(newMotorSpeed, mMotorSpeed, mNbComponents * sizeof(decimal)); memcpy(newMotorSpeed, mMotorSpeed, mNbComponents * sizeof(decimal));
memcpy(newMaxMotorTorque, mMaxMotorTorque, mNbComponents * sizeof(decimal)); memcpy(newMaxMotorForce, mMaxMotorForce, mNbComponents * sizeof(decimal));
*/ memcpy(newR2CrossN1, mR2CrossN1, mNbComponents * sizeof(decimal));
memcpy(newR2CrossN2, mR2CrossN2, mNbComponents * sizeof(decimal));
memcpy(newR2CrossSliderAxis, mR2CrossSliderAxis, mNbComponents * sizeof(decimal));
memcpy(newR1PlusUCrossN1, mR1PlusUCrossN1, mNbComponents * sizeof(decimal));
memcpy(newR1PlusUCrossN2, mR1PlusUCrossN2, mNbComponents * sizeof(decimal));
memcpy(newR1PlusUCrossSliderAxis, mR1PlusUCrossSliderAxis, mNbComponents * sizeof(decimal));
// Deallocate previous memory // Deallocate previous memory
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@ -157,16 +169,16 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
mBiasTranslation = newBiasTranslation; mBiasTranslation = newBiasTranslation;
mBiasRotation = newBiasRotation; mBiasRotation = newBiasRotation;
mInitOrientationDifferenceInv = newInitOrientationDifferenceInv; mInitOrientationDifferenceInv = newInitOrientationDifferenceInv;
/* mSliderAxisBody1 = newSliderAxisBody1;
mHingeLocalAxisBody1 = newHingeLocalAxisBody1; mSliderAxisWorld = newSliderAxisWorld;
mHingeLocalAxisBody2 = newHingeLocalAxisBody2; mR1 = newR1;
mA1 = newA1; mR2 = newR2;
mB2CrossA1 = newB2CrossA1; mN1 = newN1;
mC2CrossA1 = newC2CrossA1; mN2 = newN2;
mImpulseLowerLimit = newImpulseLowerLimit; mImpulseLowerLimit = newImpulseLowerLimit;
mImpulseUpperLimit = newImpulseUpperLimit; mImpulseUpperLimit = newImpulseUpperLimit;
mImpulseMotor = newImpulseMotor; mImpulseMotor = newImpulseMotor;
mInverseMassMatrixLimitMotor = newInverseMassMatrixLimitMotor; mInverseMassMatrixLimit = newInverseMassMatrixLimit;
mInverseMassMatrixMotor = newInverseMassMatrixMotor; mInverseMassMatrixMotor = newInverseMassMatrixMotor;
mBLowerLimit = newBLowerLimit; mBLowerLimit = newBLowerLimit;
mBUpperLimit = newBUpperLimit; mBUpperLimit = newBUpperLimit;
@ -177,8 +189,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
mIsLowerLimitViolated = newIsLowerLimitViolated; mIsLowerLimitViolated = newIsLowerLimitViolated;
mIsUpperLimitViolated = newIsUpperLimitViolated; mIsUpperLimitViolated = newIsUpperLimitViolated;
mMotorSpeed = newMotorSpeed; mMotorSpeed = newMotorSpeed;
mMaxMotorTorque = newMaxMotorTorque; mMaxMotorForce = newMaxMotorForce;
*/ mR2CrossN1 = newR2CrossN1;
mR2CrossN2 = newR2CrossN2;
mR2CrossSliderAxis = newR2CrossSliderAxis;
mR1PlusUCrossN1 = newR1PlusUCrossN1;
mR1PlusUCrossN2 = newR1PlusUCrossN2;
mR1PlusUCrossSliderAxis = newR1PlusUCrossSliderAxis;
} }
// Add a component // Add a component
@ -201,15 +218,16 @@ void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, co
new (mBiasTranslation + index) Vector2(0, 0); new (mBiasTranslation + index) Vector2(0, 0);
new (mBiasRotation + index) Vector3(0, 0, 0); new (mBiasRotation + index) Vector3(0, 0, 0);
new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0); new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0);
/* new (mSliderAxisBody1 + index) Vector3(0, 0, 0);
new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0); new (mSliderAxisWorld + index) Vector3(0, 0, 0);
new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0); new (mR1 + index) Vector3(0, 0, 0);
new (mA1 + index) Vector3(0, 0, 0); new (mR2 + index) Vector3(0, 0, 0);
new (mB2CrossA1 + index) Vector3(0, 0, 0); new (mN1 + index) Vector3(0, 0, 0);
new (mC2CrossA1 + index) Vector3(0, 0, 0); new (mN2 + index) Vector3(0, 0, 0);
mImpulseLowerLimit[index] = decimal(0.0); mImpulseLowerLimit[index] = decimal(0.0);
mImpulseUpperLimit[index] = decimal(0.0); mImpulseUpperLimit[index] = decimal(0.0);
mInverseMassMatrixLimitMotor[index] = decimal(0.0); mImpulseMotor[index] = decimal(0.0);
mInverseMassMatrixLimit[index] = decimal(0.0);
mInverseMassMatrixMotor[index] = decimal(0.0); mInverseMassMatrixMotor[index] = decimal(0.0);
mBLowerLimit[index] = decimal(0.0); mBLowerLimit[index] = decimal(0.0);
mBUpperLimit[index] = decimal(0.0); mBUpperLimit[index] = decimal(0.0);
@ -220,8 +238,13 @@ void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, co
mIsLowerLimitViolated[index] = false; mIsLowerLimitViolated[index] = false;
mIsUpperLimitViolated[index] = false; mIsUpperLimitViolated[index] = false;
mMotorSpeed[index] = component.motorSpeed; mMotorSpeed[index] = component.motorSpeed;
mMaxMotorTorque[index] = component.maxMotorTorque; mMaxMotorForce[index] = component.maxMotorForce;
*/ new (mR2CrossN1 + index) Vector3(0, 0, 0);
new (mR2CrossN2 + index) Vector3(0, 0, 0);
new (mR2CrossSliderAxis + index) Vector3(0, 0, 0);
new (mR1PlusUCrossN1 + index) Vector3(0, 0, 0);
new (mR1PlusUCrossN2 + index) Vector3(0, 0, 0);
new (mR1PlusUCrossSliderAxis + index) Vector3(0, 0, 0);
// Map the entity with the new component lookup index // Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index)); mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
@ -252,16 +275,16 @@ void SliderJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInd
new (mBiasTranslation + destIndex) Vector2(mBiasTranslation[srcIndex]); new (mBiasTranslation + destIndex) Vector2(mBiasTranslation[srcIndex]);
new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]); new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]);
new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]); new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]);
/* new (mSliderAxisBody1 + destIndex) Vector3(mSliderAxisBody1[srcIndex]);
new (mHingeLocalAxisBody1 + destIndex) Vector3(mHingeLocalAxisBody1[srcIndex]); new (mSliderAxisWorld + destIndex) Vector3(mSliderAxisWorld[srcIndex]);
new (mHingeLocalAxisBody2 + destIndex) Vector3(mHingeLocalAxisBody2[srcIndex]); new (mR1 + destIndex) Vector3(mR1[srcIndex]);
new (mA1 + destIndex) Vector3(mA1[srcIndex]); new (mR2 + destIndex) Vector3(mR2[srcIndex]);
new (mB2CrossA1 + destIndex) Vector3(mB2CrossA1[srcIndex]); new (mN1 + destIndex) Vector3(mN1[srcIndex]);
new (mC2CrossA1 + destIndex) Vector3(mC2CrossA1[srcIndex]); new (mN2 + destIndex) Vector3(mN2[srcIndex]);
mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex]; mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex];
mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex]; mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex];
mImpulseMotor[destIndex] = mImpulseMotor[srcIndex]; mImpulseMotor[destIndex] = mImpulseMotor[srcIndex];
mInverseMassMatrixLimitMotor[destIndex] = mInverseMassMatrixLimitMotor[srcIndex]; mInverseMassMatrixLimit[destIndex] = mInverseMassMatrixLimit[srcIndex];
mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex]; mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex];
mBLowerLimit[destIndex] = mBLowerLimit[srcIndex]; mBLowerLimit[destIndex] = mBLowerLimit[srcIndex];
mBUpperLimit[destIndex] = mBUpperLimit[srcIndex]; mBUpperLimit[destIndex] = mBUpperLimit[srcIndex];
@ -272,8 +295,13 @@ void SliderJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInd
mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex]; mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex];
mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[srcIndex]; mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[srcIndex];
mMotorSpeed[destIndex] = mMotorSpeed[srcIndex]; mMotorSpeed[destIndex] = mMotorSpeed[srcIndex];
mMaxMotorTorque[destIndex] = mMaxMotorTorque[srcIndex]; mMaxMotorForce[destIndex] = mMaxMotorForce[srcIndex];
*/ new (mR2CrossN1 + destIndex) Vector3(mR2CrossN1[srcIndex]);
new (mR2CrossN2 + destIndex) Vector3(mR2CrossN2[srcIndex]);
new (mR2CrossSliderAxis + destIndex) Vector3(mR2CrossSliderAxis[srcIndex]);
new (mR1PlusUCrossN1 + destIndex) Vector3(mR1PlusUCrossN1[srcIndex]);
new (mR1PlusUCrossN2 + destIndex) Vector3(mR1PlusUCrossN2[srcIndex]);
new (mR1PlusUCrossSliderAxis + destIndex) Vector3(mR1PlusUCrossSliderAxis[srcIndex]);
// Destroy the source component // Destroy the source component
destroyComponent(srcIndex); destroyComponent(srcIndex);
@ -303,16 +331,16 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) {
Vector2 biasTranslation1(mBiasTranslation[index1]); Vector2 biasTranslation1(mBiasTranslation[index1]);
Vector3 biasRotation1(mBiasRotation[index1]); Vector3 biasRotation1(mBiasRotation[index1]);
Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]); Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]);
/* Vector3 sliderAxisBody1(mSliderAxisBody1[index1]);
Vector3 hingeLocalAxisBody1(mHingeLocalAxisBody1[index1]); Vector3 sliderAxisWorld(mSliderAxisWorld[index1]);
Vector3 hingeLocalAxisBody2(mHingeLocalAxisBody2[index1]); Vector3 r1(mR1[index1]);
Vector3 a1(mA1[index1]); Vector3 r2(mR2[index1]);
Vector3 b2CrossA1(mB2CrossA1[index1]); Vector3 n1(mN1[index1]);
Vector3 c2CrossA1(mC2CrossA1[index1]); Vector3 n2(mN2[index1]);
decimal impulseLowerLimit(mImpulseLowerLimit[index1]); decimal impulseLowerLimit(mImpulseLowerLimit[index1]);
decimal impulseUpperLimit(mImpulseUpperLimit[index1]); decimal impulseUpperLimit(mImpulseUpperLimit[index1]);
decimal impulseMotor(mImpulseMotor[index1]); decimal impulseMotor(mImpulseMotor[index1]);
decimal inverseMassMatrixLimitMotor(mInverseMassMatrixLimitMotor[index1]); decimal inverseMassMatrixLimit(mInverseMassMatrixLimit[index1]);
decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]); decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]);
decimal bLowerLimit(mBLowerLimit[index1]); decimal bLowerLimit(mBLowerLimit[index1]);
decimal bUpperLimit(mUpperLimit[index1]); decimal bUpperLimit(mUpperLimit[index1]);
@ -323,8 +351,13 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) {
bool isLowerLimitViolated(mIsLowerLimitViolated[index1]); bool isLowerLimitViolated(mIsLowerLimitViolated[index1]);
bool isUpperLimitViolated(mIsUpperLimitViolated[index1]); bool isUpperLimitViolated(mIsUpperLimitViolated[index1]);
decimal motorSpeed(mMotorSpeed[index1]); decimal motorSpeed(mMotorSpeed[index1]);
decimal maxMotorTorque(mMaxMotorTorque[index1]); decimal maxMotorForce(mMaxMotorForce[index1]);
*/ Vector3 r2CrossN1(mR2CrossN1[index1]);
Vector3 r2CrossN2(mR2CrossN2[index1]);
Vector3 r2CrossSliderAxis(mR2CrossSliderAxis[index1]);
Vector3 r1PlusUCrossN1(mR1PlusUCrossN1[index1]);
Vector3 r1PlusUCrossN2(mR1PlusUCrossN2[index1]);
Vector3 r1PlusUCrossSliderAxis(mR1PlusUCrossSliderAxis[index1]);
// Destroy component 1 // Destroy component 1
destroyComponent(index1); destroyComponent(index1);
@ -345,16 +378,16 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) {
new (mBiasTranslation + index2) Vector2(biasTranslation1); new (mBiasTranslation + index2) Vector2(biasTranslation1);
new (mBiasRotation + index2) Vector3(biasRotation1); new (mBiasRotation + index2) Vector3(biasRotation1);
new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1);
/* new (mSliderAxisBody1 + index2) Vector3(sliderAxisBody1);
new (mHingeLocalAxisBody1 + index2) Vector3(hingeLocalAxisBody1); new (mSliderAxisWorld + index2) Vector3(sliderAxisWorld);
new (mHingeLocalAxisBody2 + index2) Vector3(hingeLocalAxisBody2); new (mR1 + index2) Vector3(r1);
new (mA1 + index2) Vector3(a1); new (mR2 + index2) Vector3(r2);
new (mB2CrossA1 + index2) Vector3(b2CrossA1); new (mN1 + index2) Vector3(n1);
new (mC2CrossA1 + index2) Vector3(c2CrossA1); new (mN2 + index2) Vector3(n2);
mImpulseLowerLimit[index2] = impulseLowerLimit; mImpulseLowerLimit[index2] = impulseLowerLimit;
mImpulseUpperLimit[index2] = impulseUpperLimit; mImpulseUpperLimit[index2] = impulseUpperLimit;
mImpulseMotor[index2] = impulseMotor; mImpulseMotor[index2] = impulseMotor;
mInverseMassMatrixLimitMotor[index2] = inverseMassMatrixLimitMotor; mInverseMassMatrixLimit[index2] = inverseMassMatrixLimit;
mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor; mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor;
mBLowerLimit[index2] = bLowerLimit; mBLowerLimit[index2] = bLowerLimit;
mBUpperLimit[index2] = bUpperLimit; mBUpperLimit[index2] = bUpperLimit;
@ -365,8 +398,13 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) {
mIsLowerLimitViolated[index2] = isLowerLimitViolated; mIsLowerLimitViolated[index2] = isLowerLimitViolated;
mIsUpperLimitViolated[index2] = isUpperLimitViolated; mIsUpperLimitViolated[index2] = isUpperLimitViolated;
mMotorSpeed[index2] = motorSpeed; mMotorSpeed[index2] = motorSpeed;
mMaxMotorTorque[index2] = maxMotorTorque; mMaxMotorForce[index2] = maxMotorForce;
*/ new (mR2CrossN1 + index2) Vector3(r2CrossN1);
new (mR2CrossN2 + index2) Vector3(r2CrossN2);
new (mR2CrossSliderAxis + index2) Vector3(r2CrossSliderAxis);
new (mR1PlusUCrossN1 + index2) Vector3(r1PlusUCrossN1);
new (mR1PlusUCrossN2 + index2) Vector3(r1PlusUCrossN2);
new (mR1PlusUCrossSliderAxis + index2) Vector3(r1PlusUCrossSliderAxis);
// Update the entity to component index mapping // Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2)); mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));
@ -398,11 +436,16 @@ void SliderJointComponents::destroyComponent(uint32 index) {
mBiasTranslation[index].~Vector2(); mBiasTranslation[index].~Vector2();
mBiasRotation[index].~Vector3(); mBiasRotation[index].~Vector3();
mInitOrientationDifferenceInv[index].~Quaternion(); mInitOrientationDifferenceInv[index].~Quaternion();
/* mSliderAxisBody1[index].~Vector3();
mHingeLocalAxisBody1[index].~Vector3(); mSliderAxisWorld[index].~Vector3();
mHingeLocalAxisBody2[index].~Vector3(); mR1[index].~Vector3();
mA1[index].~Vector3(); mR2[index].~Vector3();
mB2CrossA1[index].~Vector3(); mN1[index].~Vector3();
mC2CrossA1[index].~Vector3(); mN2[index].~Vector3();
*/ mR2CrossN1[index].~Vector3();
mR2CrossN2[index].~Vector3();
mR2CrossSliderAxis[index].~Vector3();
mR1PlusUCrossN1[index].~Vector3();
mR1PlusUCrossN2[index].~Vector3();
mR1PlusUCrossSliderAxis[index].~Vector3();
} }

396
src/components/SliderJointComponents.h
View File

@ -92,21 +92,23 @@ class SliderJointComponents : public Components {
/// Inverse of the initial orientation difference between the two bodies /// Inverse of the initial orientation difference between the two bodies
Quaternion* mInitOrientationDifferenceInv; Quaternion* mInitOrientationDifferenceInv;
/* /// Slider axis (in local-space coordinates of body 1)
/// Hinge rotation axis (in local-space coordinates of body 1) Vector3* mSliderAxisBody1;
Vector3* mHingeLocalAxisBody1;
/// Hinge rotation axis (in local-space coordiantes of body 2) /// Slider axis in world-space coordinates
Vector3* mHingeLocalAxisBody2; Vector3* mSliderAxisWorld;
/// Hinge rotation axis (in world-space coordinates) computed from body 1 /// Vector r1 in world-space coordinates
Vector3* mA1; Vector3* mR1;
/// Cross product of vector b2 and a1 /// Vector r2 in world-space coordinates
Vector3* mB2CrossA1; Vector3* mR2;
/// Cross product of vector c2 and a1; /// First vector orthogonal to the slider axis local-space of body 1
Vector3* mC2CrossA1; Vector3* mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3* mN2;
/// Accumulated impulse for the lower limit constraint /// Accumulated impulse for the lower limit constraint
decimal* mImpulseLowerLimit; decimal* mImpulseLowerLimit;
@ -117,8 +119,8 @@ class SliderJointComponents : public Components {
/// Accumulated impulse for the motor constraint; /// Accumulated impulse for the motor constraint;
decimal* mImpulseMotor; decimal* mImpulseMotor;
/// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
decimal* mInverseMassMatrixLimitMotor; decimal* mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor /// Inverse of mass matrix K=JM^-1J^t for the motor
decimal* mInverseMassMatrixMotor; decimal* mInverseMassMatrixMotor;
@ -150,10 +152,26 @@ class SliderJointComponents : public Components {
/// Motor speed (in rad/s) /// Motor speed (in rad/s)
decimal* mMotorSpeed; decimal* mMotorSpeed;
/// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal* mMaxMotorTorque; decimal* mMaxMotorForce;
*/ /// Cross product of r2 and n1
Vector3* mR2CrossN1;
/// Cross product of r2 and n2
Vector3* mR2CrossN2;
/// Cross product of r2 and the slider axis
Vector3* mR2CrossSliderAxis;
/// Cross product of vector (r1 + u) and n1
Vector3* mR1PlusUCrossN1;
/// Cross product of vector (r1 + u) and n2
Vector3* mR1PlusUCrossN2;
/// Cross product of vector (r1 + u) and the slider axis
Vector3* mR1PlusUCrossSliderAxis;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -179,18 +197,13 @@ class SliderJointComponents : public Components {
decimal lowerLimit; decimal lowerLimit;
decimal upperLimit; decimal upperLimit;
decimal motorSpeed; decimal motorSpeed;
decimal maxMotorTorque; decimal maxMotorForce;
// TODO : Delete this
SliderJointComponent() {
}
/// Constructor /// Constructor
SliderJointComponent(bool isLimitEnabled, bool isMotorEnabled, decimal lowerLimit, decimal upperLimit, SliderJointComponent(bool isLimitEnabled, bool isMotorEnabled, decimal lowerLimit, decimal upperLimit,
decimal motorSpeed, decimal maxMotorTorque) decimal motorSpeed, decimal maxMotorForce)
: isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit), :isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit),
motorSpeed(motorSpeed), maxMotorTorque(maxMotorTorque) { motorSpeed(motorSpeed), maxMotorForce(maxMotorForce) {
} }
}; };
@ -278,36 +291,41 @@ class SliderJointComponents : public Components {
/// Set the rotation impulse /// Set the rotation impulse
void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv); void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv);
/* /// Return the slider axis (in local-space coordinates of body 1)
/// Return the hinge rotation axis (in local-space coordinates of body 1) Vector3& getSliderAxisBody1(Entity jointEntity);
Vector3& getHingeLocalAxisBody1(Entity jointEntity);
/// Set the hinge rotation axis (in local-space coordinates of body 1) /// Set the slider axis (in local-space coordinates of body 1)
void setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1); void setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1);
/// Return the hinge rotation axis (in local-space coordiantes of body 2) /// Retunr the slider axis in world-space coordinates
Vector3& getHingeLocalAxisBody2(Entity jointEntity); Vector3& getSliderAxisWorld(Entity jointEntity);
/// Set the hinge rotation axis (in local-space coordiantes of body 2) /// Set the slider axis in world-space coordinates
void setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2); void setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld);
/// Return the hinge rotation axis (in world-space coordinates) computed from body 1 /// Return the vector r1 in world-space coordinates
Vector3& getA1(Entity jointEntity); Vector3& getR1(Entity jointEntity);
/// Set the hinge rotation axis (in world-space coordinates) computed from body 1 /// Set the vector r1 in world-space coordinates
void setA1(Entity jointEntity, const Vector3& a1); void setR1(Entity jointEntity, const Vector3& r1);
/// Return the cross product of vector b2 and a1 /// Return the vector r2 in world-space coordinates
Vector3& getB2CrossA1(Entity jointEntity); Vector3& getR2(Entity jointEntity);
/// Set the cross product of vector b2 and a1 /// Set the vector r2 in world-space coordinates
void setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1); void setR2(Entity jointEntity, const Vector3& r2);
/// Return the cross product of vector c2 and a1; /// Return the first vector orthogonal to the slider axis local-space of body 1
Vector3& getC2CrossA1(Entity jointEntity); Vector3& getN1(Entity jointEntity);
/// Set the cross product of vector c2 and a1; /// Set the first vector orthogonal to the slider axis local-space of body 1
void setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1); void setN1(Entity jointEntity, const Vector3& n1);
/// Return the second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3& getN2(Entity jointEntity);
/// Set the second vector orthogonal to the slider axis and mN1 in local-space of body 1
void setN2(Entity jointEntity, const Vector3& n2);
/// Return the accumulated impulse for the lower limit constraint /// Return the accumulated impulse for the lower limit constraint
decimal getImpulseLowerLimit(Entity jointEntity) const; decimal getImpulseLowerLimit(Entity jointEntity) const;
@ -327,11 +345,11 @@ class SliderJointComponents : public Components {
/// Set the accumulated impulse for the motor constraint; /// Set the accumulated impulse for the motor constraint;
void setImpulseMotor(Entity jointEntity, decimal impulseMotor); void setImpulseMotor(Entity jointEntity, decimal impulseMotor);
/// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Return the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix)
decimal getInverseMassMatrixLimitMotor(Entity jointEntity) const; decimal getInverseMassMatrixLimit(Entity jointEntity) const;
/// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) /// Set the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix)
void setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor); void setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor);
/// Return the inverse of mass matrix K=JM^-1J^t for the motor /// Return the inverse of mass matrix K=JM^-1J^t for the motor
decimal getInverseMassMatrixMotor(Entity jointEntity); decimal getInverseMassMatrixMotor(Entity jointEntity);
@ -393,12 +411,47 @@ class SliderJointComponents : public Components {
/// Set the motor speed (in rad/s) /// Set the motor speed (in rad/s)
void setMotorSpeed(Entity jointEntity, decimal motorSpeed); void setMotorSpeed(Entity jointEntity, decimal motorSpeed);
/// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Return the maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal getMaxMotorTorque(Entity jointEntity) const; decimal getMaxMotorForce(Entity jointEntity) const;
/// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Set the maximum motor force (in Newtons) that can be applied to reach to desired motor speed
void setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque); void setMaxMotorForce(Entity jointEntity, decimal maxMotorForce);
*/
/// Return the cross product of r2 and n1
Vector3& getR2CrossN1(Entity jointEntity);
/// Set the cross product of r2 and n1
void setR2CrossN1(Entity jointEntity, const Vector3& r2CrossN1);
/// Return the cross product of r2 and n2
Vector3& getR2CrossN2(Entity jointEntity);
/// Set the cross product of r2 and n2
void setR2CrossN2(Entity jointEntity, const Vector3& r2CrossN2);
/// Return the cross product of r2 and the slider axis
Vector3& getR2CrossSliderAxis(Entity jointEntity);
/// Set the cross product of r2 and the slider axis
void setR2CrossSliderAxis(Entity jointEntity, const Vector3& r2CrossSliderAxis);
/// Return the cross product of vector (r1 + u) and n1
Vector3& getR1PlusUCrossN1(Entity jointEntity);
/// Set the cross product of vector (r1 + u) and n1
void setR1PlusUCrossN1(Entity jointEntity, const Vector3& r1PlusUCrossN1);
/// Return the cross product of vector (r1 + u) and n2
Vector3& getR1PlusUCrossN2(Entity jointEntity);
/// Set the cross product of vector (r1 + u) and n2
void setR1PlusUCrossN2(Entity jointEntity, const Vector3& r1PlusUCrossN2);
/// Return the cross product of vector (r1 + u) and the slider axis
Vector3& getR1PlusUCrossSliderAxis(Entity jointEntity);
/// Set the cross product of vector (r1 + u) and the slider axis
void setR1PlusUCrossSliderAxis(Entity jointEntity, const Vector3& r1PlusUCrossSliderAxis);
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
@ -573,87 +626,99 @@ inline void SliderJointComponents::setInitOrientationDifferenceInv(Entity jointE
mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv;
} }
/* // Return the slider axis (in local-space coordinates of body 1)
// Return the hinge rotation axis (in local-space coordinates of body 1) inline Vector3& SliderJointComponents::getSliderAxisBody1(Entity jointEntity) {
inline Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]]; return mSliderAxisBody1[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the hinge rotation axis (in local-space coordinates of body 1) // Set the slider axis (in local-space coordinates of body 1)
inline void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) { inline void SliderJointComponents::setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody1; mSliderAxisBody1[mMapEntityToComponentIndex[jointEntity]] = sliderAxisBody1;
} }
// Return the hinge rotation axis (in local-space coordiantes of body 2) // Retunr the slider axis in world-space coordinates
inline Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) { inline Vector3& SliderJointComponents::getSliderAxisWorld(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]]; return mSliderAxisWorld[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the hinge rotation axis (in local-space coordiantes of body 2) // Set the slider axis in world-space coordinates
inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) { inline void SliderJointComponents::setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody2; mSliderAxisWorld[mMapEntityToComponentIndex[jointEntity]] = sliderAxisWorld;
} }
// Return the vector r1 in world-space coordinates
// Return the hinge rotation axis (in world-space coordinates) computed from body 1 inline Vector3& SliderJointComponents::getR1(Entity jointEntity) {
inline Vector3& HingeJointComponents::getA1(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mA1[mMapEntityToComponentIndex[jointEntity]]; return mR1[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the hinge rotation axis (in world-space coordinates) computed from body 1 // Set the vector r1 in world-space coordinates
inline void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) { inline void SliderJointComponents::setR1(Entity jointEntity, const Vector3& r1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mA1[mMapEntityToComponentIndex[jointEntity]] = a1; mR1[mMapEntityToComponentIndex[jointEntity]] = r1;
} }
// Return the cross product of vector b2 and a1 // Return the vector r2 in world-space coordinates
inline Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) { inline Vector3& SliderJointComponents::getR2(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mB2CrossA1[mMapEntityToComponentIndex[jointEntity]]; return mR2[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the cross product of vector b2 and a1 // Set the vector r2 in world-space coordinates
inline void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) { inline void SliderJointComponents::setR2(Entity jointEntity, const Vector3& r2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mB2CrossA1[mMapEntityToComponentIndex[jointEntity]] = b2CrossA1; mR2[mMapEntityToComponentIndex[jointEntity]] = r2;
} }
// Return the cross product of vector c2 and a1; // Return the first vector orthogonal to the slider axis local-space of body 1
inline Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) { inline Vector3& SliderJointComponents::getN1(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mC2CrossA1[mMapEntityToComponentIndex[jointEntity]]; return mN1[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the cross product of vector c2 and a1; // Set the first vector orthogonal to the slider axis local-space of body 1
inline void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) { inline void SliderJointComponents::setN1(Entity jointEntity, const Vector3& n1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mC2CrossA1[mMapEntityToComponentIndex[jointEntity]] = c2CrossA1; mN1[mMapEntityToComponentIndex[jointEntity]] = n1;
}
// Return the second vector orthogonal to the slider axis and mN1 in local-space of body 1
inline Vector3& SliderJointComponents::getN2(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mN2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the second vector orthogonal to the slider axis and mN1 in local-space of body 1
inline void SliderJointComponents::setN2(Entity jointEntity, const Vector3& n2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mN2[mMapEntityToComponentIndex[jointEntity]] = n2;
} }
// Return the accumulated impulse for the lower limit constraint // Return the accumulated impulse for the lower limit constraint
inline decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getImpulseLowerLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]]; return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the accumulated impulse for the lower limit constraint // Set the accumulated impulse for the lower limit constraint
inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { inline void SliderJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit; mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit;
@ -661,14 +726,14 @@ inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decim
// Return the accumulated impulse for the upper limit constraint // Return the accumulated impulse for the upper limit constraint
inline decimal HingeJointComponents::getImpulseUpperLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getImpulseUpperLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]]; return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the accumulated impulse for the upper limit constraint // Set the accumulated impulse for the upper limit constraint
inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const { inline void SliderJointComponents::setImpulseUpperLimit(Entity jointEntity, decimal impulseUpperLimit) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit; mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit;
@ -676,187 +741,270 @@ inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decim
// Return the accumulated impulse for the motor constraint; // Return the accumulated impulse for the motor constraint;
inline decimal HingeJointComponents::getImpulseMotor(Entity jointEntity) const { inline decimal SliderJointComponents::getImpulseMotor(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]]; return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the accumulated impulse for the motor constraint; // Set the accumulated impulse for the motor constraint;
inline void HingeJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) { inline void SliderJointComponents::setImpulseMotor(Entity jointEntity, decimal impulseMotor) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor; mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor;
} }
// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) // Return the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix)
inline decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const { inline decimal SliderJointComponents::getInverseMassMatrixLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]]; return mInverseMassMatrixLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) // Set the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix)
inline void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { inline void SliderJointComponents::setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mInverseMassMatrixLimitMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor; mInverseMassMatrixLimit[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixLimitMotor;
} }
// Return the inverse of mass matrix K=JM^-1J^t for the motor // Return the inverse of mass matrix K=JM^-1J^t for the motor
inline decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { inline decimal SliderJointComponents::getInverseMassMatrixMotor(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]]; return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]];
} }
// Return the inverse of mass matrix K=JM^-1J^t for the motor // Return the inverse of mass matrix K=JM^-1J^t for the motor
inline void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { inline void SliderJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor; mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor;
} }
// Return the bias of the lower limit constraint // Return the bias of the lower limit constraint
inline decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getBLowerLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]]; return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the bias of the lower limit constraint // Set the bias of the lower limit constraint
inline void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { inline void SliderJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit; mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit;
} }
// Return the bias of the upper limit constraint // Return the bias of the upper limit constraint
inline decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getBUpperLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]]; return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the bias of the upper limit constraint // Set the bias of the upper limit constraint
inline void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { inline void SliderJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit; mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit;
} }
// Return true if the joint limits are enabled // Return true if the joint limits are enabled
inline bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const { inline bool SliderJointComponents::getIsLimitEnabled(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]]; return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]];
} }
// Set to true if the joint limits are enabled // Set to true if the joint limits are enabled
inline void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { inline void SliderJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled; mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled;
} }
// Return true if the motor of the joint in enabled // Return true if the motor of the joint in enabled
inline bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const { inline bool SliderJointComponents::getIsMotorEnabled(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]]; return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]];
} }
// Set to true if the motor of the joint in enabled // Set to true if the motor of the joint in enabled
inline void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { inline void SliderJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled; mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled;
} }
// Return the Lower limit (minimum allowed rotation angle in radian) // Return the Lower limit (minimum allowed rotation angle in radian)
inline decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getLowerLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mLowerLimit[mMapEntityToComponentIndex[jointEntity]]; return mLowerLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the Lower limit (minimum allowed rotation angle in radian) // Set the Lower limit (minimum allowed rotation angle in radian)
inline void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { inline void SliderJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit; mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit;
} }
// Return the upper limit (maximum translation distance) // Return the upper limit (maximum translation distance)
inline decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const { inline decimal SliderJointComponents::getUpperLimit(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mUpperLimit[mMapEntityToComponentIndex[jointEntity]]; return mUpperLimit[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the upper limit (maximum translation distance) // Set the upper limit (maximum translation distance)
inline void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { inline void SliderJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit; mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit;
} }
// Return true if the lower limit is violated // Return true if the lower limit is violated
inline bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { inline bool SliderJointComponents::getIsLowerLimitViolated(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]]; return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]];
} }
// Set to true if the lower limit is violated // Set to true if the lower limit is violated
inline void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { inline void SliderJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated; mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated;
} }
// Return true if the upper limit is violated // Return true if the upper limit is violated
inline bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { inline bool SliderJointComponents::getIsUpperLimitViolated(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]]; return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]];
} }
// Set to true if the upper limit is violated // Set to true if the upper limit is violated
inline void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { inline void SliderJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated; mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated;
} }
// Return the motor speed (in rad/s) // Return the motor speed (in rad/s)
inline decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const { inline decimal SliderJointComponents::getMotorSpeed(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]]; return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the motor speed (in rad/s) // Set the motor speed (in rad/s)
inline void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { inline void SliderJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed; mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed;
} }
// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed // Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
inline decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const { inline decimal SliderJointComponents::getMaxMotorForce(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]]; return mMaxMotorForce[mMapEntityToComponentIndex[jointEntity]];
} }
// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed // 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) { inline void SliderJointComponents::setMaxMotorForce(Entity jointEntity, decimal maxMotorForce) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity)); assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mMaxMotorTorque[mMapEntityToComponentIndex[jointEntity]] = maxMotorTorque; mMaxMotorForce[mMapEntityToComponentIndex[jointEntity]] = maxMotorForce;
}
// Return the cross product of r2 and n1
inline Vector3& SliderJointComponents::getR2CrossN1(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR2CrossN1[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of r2 and n1
inline void SliderJointComponents::setR2CrossN1(Entity jointEntity, const Vector3& r2CrossN1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR2CrossN1[mMapEntityToComponentIndex[jointEntity]] = r2CrossN1;
}
// Return the cross product of r2 and n2
inline Vector3& SliderJointComponents::getR2CrossN2(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR2CrossN2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of r2 and n2
inline void SliderJointComponents::setR2CrossN2(Entity jointEntity, const Vector3& r2CrossN2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR2CrossN2[mMapEntityToComponentIndex[jointEntity]] = r2CrossN2;
}
// Return the cross product of r2 and the slider axis
inline Vector3& SliderJointComponents::getR2CrossSliderAxis(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR2CrossSliderAxis[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of r2 and the slider axis
inline void SliderJointComponents::setR2CrossSliderAxis(Entity jointEntity, const Vector3& r2CrossSliderAxis) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR2CrossSliderAxis[mMapEntityToComponentIndex[jointEntity]] = r2CrossSliderAxis;
}
// Return the cross product of vector (r1 + u) and n1
inline Vector3& SliderJointComponents::getR1PlusUCrossN1(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR1PlusUCrossN1[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of vector (r1 + u) and n1
inline void SliderJointComponents::setR1PlusUCrossN1(Entity jointEntity, const Vector3& r1PlusUCrossN1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR1PlusUCrossN1[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossN1;
}
// Return the cross product of vector (r1 + u) and n2
inline Vector3& SliderJointComponents::getR1PlusUCrossN2(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR1PlusUCrossN2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of vector (r1 + u) and n2
inline void SliderJointComponents::setR1PlusUCrossN2(Entity jointEntity, const Vector3& r1PlusUCrossN2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR1PlusUCrossN2[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossN2;
}
// Return the cross product of vector (r1 + u) and the slider axis
inline Vector3& SliderJointComponents::getR1PlusUCrossSliderAxis(Entity jointEntity) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mR1PlusUCrossSliderAxis[mMapEntityToComponentIndex[jointEntity]];
}
// Set the cross product of vector (r1 + u) and the slider axis
inline void SliderJointComponents::setR1PlusUCrossSliderAxis(Entity jointEntity, const Vector3& r1PlusUCrossSliderAxis) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mR1PlusUCrossSliderAxis[mMapEntityToComponentIndex[jointEntity]] = r1PlusUCrossSliderAxis;
} }
*/
} }

504
src/constraint/SliderJoint.cpp
View File

@ -36,16 +36,11 @@ const decimal SliderJoint::BETA = decimal(0.2);
// Constructor // Constructor
SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo) SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), : Joint(entity, world, jointInfo) {
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit),
mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false),
mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed),
mMaxMotorForce(jointInfo.maxMotorForce){
assert(mUpperLimit >= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getUpperLimit(mEntity) >= decimal(0.0));
assert(mLowerLimit <= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getLowerLimit(mEntity) <= decimal(0.0));
assert(mMaxMotorForce >= decimal(0.0)); assert(mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) >= decimal(0.0));
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
@ -69,9 +64,10 @@ SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointI
// Compute the slider axis in local-space of body 1 // Compute the slider axis in local-space of body 1
// TODO : Do not compute the inverse here, it has already been computed above // TODO : Do not compute the inverse here, it has already been computed above
mSliderAxisBody1 = transform1.getOrientation().getInverse() * Vector3 sliderAxisBody1 = transform1.getOrientation().getInverse() *
jointInfo.sliderAxisWorldSpace; jointInfo.sliderAxisWorldSpace;
mSliderAxisBody1.normalize(); sliderAxisBody1.normalize();
mWorld.mSliderJointsComponents.setSliderAxisBody1(mEntity, sliderAxisBody1);
} }
// Initialize before solving the constraint // Initialize before solving the constraint
@ -96,41 +92,56 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
mR2 = orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); mWorld.mSliderJointsComponents.setR2(mEntity, orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity));
const Vector3& r1 = mWorld.mSliderJointsComponents.getR1(mEntity);
const Vector3& r2 = mWorld.mSliderJointsComponents.getR2(mEntity);
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1; const Vector3 u = x2 + r2 - x1 - r1;
// Compute the two orthogonal vectors to the slider axis in world-space // Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; Vector3 sliderAxisWorld = orientationBody1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity);
mSliderAxisWorld.normalize(); sliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld);
mN2 = mSliderAxisWorld.cross(mN1); mWorld.mSliderJointsComponents.setN1(mEntity, sliderAxisWorld.getOneUnitOrthogonalVector());
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
mWorld.mSliderJointsComponents.setN2(mEntity, sliderAxisWorld.cross(n1));
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld); decimal uDotSliderAxis = u.dot(sliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit; decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity);
decimal upperLimitError = mUpperLimit - uDotSliderAxis; decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis;
bool oldIsLowerLimitViolated = mIsLowerLimitViolated; bool oldIsLowerLimitViolated = mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity);
mIsLowerLimitViolated = lowerLimitError <= 0; bool isLowerLimitViolated = lowerLimitError <= 0;
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated);
mImpulseLowerLimit = 0.0; if (isLowerLimitViolated != oldIsLowerLimitViolated) {
mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
} }
bool oldIsUpperLimitViolated = mIsUpperLimitViolated; bool oldIsUpperLimitViolated = mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity);
mIsUpperLimitViolated = upperLimitError <= 0; bool isUpperLimitViolated = upperLimitError <= 0;
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated);
mImpulseUpperLimit = 0.0; if (isUpperLimitViolated != oldIsUpperLimitViolated) {
mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
} }
// Compute the cross products used in the Jacobians // Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1); mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1));
mR2CrossN2 = mR2.cross(mN2); mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2));
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld));
const Vector3 r1PlusU = mR1 + u; const Vector3 r1PlusU = r1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1));
mR1PlusUCrossN2 = (r1PlusU).cross(mN2); mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2));
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); mWorld.mSliderJointsComponents.setR1PlusUCrossSliderAxis(mEntity, r1PlusU.cross(sliderAxisWorld));
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity); const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity);
@ -140,18 +151,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const decimal sumInverseMass = body1MassInverse + body2MassInverse; const decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
Vector3 I2R2CrossN1 = i2 * mR2CrossN1; Vector3 I2R2CrossN1 = i2 * r2CrossN1;
Vector3 I2R2CrossN2 = i2 * mR2CrossN2; Vector3 I2R2CrossN2 = i2 * r2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); r2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); r2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); r2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); r2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero(); inverseMassMatrixTranslation.setToZero();
@ -166,8 +177,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
biasTranslation.setToZero(); biasTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep); decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
biasTranslation.x = u.dot(mN1); biasTranslation.x = u.dot(n1);
biasTranslation.y = u.dot(mN2); biasTranslation.y = u.dot(n2);
biasTranslation *= biasFactor; biasTranslation *= biasFactor;
mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation); mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation);
} }
@ -190,35 +201,38 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
} }
// If the limits are enabled // If the limits are enabled
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity) ||
mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity))) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = sumInverseMass + decimal inverseMassMatrixLimit = sumInverseMass +
mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); decimal(1.0) / inverseMassMatrixLimit : decimal(0.0);
mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit);
// Compute the bias "b" of the lower limit constraint // Compute the bias "b" of the lower limit constraint
mBLowerLimit = 0.0; mWorld.mSliderJointsComponents.setBLowerLimit(mEntity, decimal(0.0));
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mBLowerLimit = biasFactor * lowerLimitError; mWorld.mSliderJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError);
} }
// Compute the bias "b" of the upper limit constraint // Compute the bias "b" of the upper limit constraint
mBUpperLimit = 0.0; mWorld.mSliderJointsComponents.setBUpperLimit(mEntity, decimal(0.0));
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mBUpperLimit = biasFactor * upperLimitError; mWorld.mSliderJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError);
} }
} }
// If the motor is enabled // If the motor is enabled
if (mIsMotorEnabled) { if (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity)) {
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = sumInverseMass; decimal inverseMassMatrixMotor = sumInverseMass;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > decimal(0.0)) ? inverseMassMatrixMotor = (inverseMassMatrixMotor > decimal(0.0)) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); decimal(1.0) / inverseMassMatrixMotor : decimal(0.0);
mWorld.mSliderJointsComponents.setInverseMassMatrixMotor(mEntity, inverseMassMatrixMotor);
} }
// If warm-starting is not enabled // If warm-starting is not enabled
@ -229,9 +243,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity);
impulseTranslation.setToZero(); impulseTranslation.setToZero();
impulseRotation.setToZero(); impulseRotation.setToZero();
mImpulseLowerLimit = 0.0; mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
mImpulseUpperLimit = 0.0; mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
mImpulseMotor = 0.0; mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
} }
} }
@ -255,27 +269,30 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; decimal impulseLimits = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity);
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; Vector3 linearImpulseLimits = impulseLimits * mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
// Compute the impulse P=J^T * lambda for the motor constraint of body 1 // Compute the impulse P=J^T * lambda for the motor constraint of body 1
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld; Vector3 impulseMotor = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) * mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
const Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity); const Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity);
const Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); const Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
Vector3 linearImpulseBody1 = -mN1 * impulseTranslation.x - mN2 * impulseTranslation.y; Vector3 linearImpulseBody1 = -n1 * impulseTranslation.x - n2 * impulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * impulseTranslation.x - Vector3 angularImpulseBody1 = -mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity) * impulseTranslation.x -
mR1PlusUCrossN2 * impulseTranslation.y; mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity) * impulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 += -impulseRotation; angularImpulseBody1 += -impulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
linearImpulseBody1 += linearImpulseLimits; linearImpulseBody1 += linearImpulseLimits;
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis; angularImpulseBody1 += impulseLimits * mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
// Compute the impulse P=J^T * lambda for the motor constraint of body 1 // Compute the impulse P=J^T * lambda for the motor constraint of body 1
linearImpulseBody1 += impulseMotor; linearImpulseBody1 += impulseMotor;
@ -285,16 +302,16 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
w1 += mWorld.mSliderJointsComponents.getI1(mEntity) * angularImpulseBody1; w1 += mWorld.mSliderJointsComponents.getI1(mEntity) * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
Vector3 linearImpulseBody2 = mN1 * impulseTranslation.x + mN2 * impulseTranslation.y; Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * impulseTranslation.x + Vector3 angularImpulseBody2 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity) * impulseTranslation.x +
mR2CrossN2 * impulseTranslation.y; mWorld.mSliderJointsComponents.getR2CrossN2(mEntity) * impulseTranslation.y;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
angularImpulseBody2 += impulseRotation; angularImpulseBody2 += impulseRotation;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
linearImpulseBody2 += -linearImpulseLimits; linearImpulseBody2 += -linearImpulseLimits;
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis; angularImpulseBody2 += -impulseLimits * mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
// Compute the impulse P=J^T * lambda for the motor constraint of body 2 // Compute the impulse P=J^T * lambda for the motor constraint of body 2
linearImpulseBody2 += -impulseMotor; linearImpulseBody2 += -impulseMotor;
@ -323,17 +340,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity); const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity);
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& sliderAxisWorld = mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 2 translation constraints // Compute J*v for the 2 translation constraints
const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) +
mN1.dot(v2) + w2.dot(mR2CrossN1); n1.dot(v2) + w2.dot(r2CrossN1);
const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) +
mN2.dot(v2) + w2.dot(mR2CrossN2); n2.dot(v2) + w2.dot(r2CrossN2);
const Vector2 JvTranslation(el1, el2); const Vector2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints // Compute the Lagrange multiplier lambda for the 2 translation constraints
@ -341,17 +370,17 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
mWorld.mSliderJointsComponents.setImpulseTranslation(mEntity, deltaLambda + mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity)); mWorld.mSliderJointsComponents.setImpulseTranslation(mEntity, deltaLambda + mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity));
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x -
mR1PlusUCrossN2 * deltaLambda.y; r1PlusUCrossN2 * deltaLambda.y;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1; w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -381,32 +410,34 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) { if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity)) {
const decimal inverseMassMatrixLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity);
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) {
// Compute J*v for the lower limit constraint // Compute J*v for the lower limit constraint
const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - const decimal JvLowerLimit = sliderAxisWorld.dot(v2) + r2CrossSliderAxis.dot(w2) -
mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); sliderAxisWorld.dot(v1) - r1PlusUCrossSliderAxis.dot(w1);
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); decimal deltaLambdaLower = inverseMassMatrixLimit * (-JvLowerLimit - mWorld.mSliderJointsComponents.getBLowerLimit(mEntity));
decimal lambdaTemp = mImpulseLowerLimit; decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity);
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) + deltaLambdaLower, decimal(0.0)));
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; deltaLambdaLower = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -deltaLambdaLower * sliderAxisWorld;
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; const Vector3 angularImpulseBody1 = -deltaLambdaLower * r1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1; w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld;
const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; const Vector3 angularImpulseBody2 = deltaLambdaLower * r2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -414,29 +445,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
// Compute J*v for the upper limit constraint // Compute J*v for the upper limit constraint
const decimal JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) const decimal JvUpperLimit = sliderAxisWorld.dot(v1) + r1PlusUCrossSliderAxis.dot(w1)
- mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); - sliderAxisWorld.dot(v2) - r2CrossSliderAxis.dot(w2);
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); decimal deltaLambdaUpper = inverseMassMatrixLimit * (-JvUpperLimit -mWorld.mSliderJointsComponents.getBUpperLimit(mEntity));
decimal lambdaTemp = mImpulseUpperLimit; decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity);
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) + deltaLambdaUpper, decimal(0.0)));
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; deltaLambdaUpper = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaUpper * sliderAxisWorld;
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; const Vector3 angularImpulseBody1 = deltaLambdaUpper * r1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1; w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld;
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; const Vector3 angularImpulseBody2 = -deltaLambdaUpper * r2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -446,26 +477,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// --------------- Motor --------------- // // --------------- Motor --------------- //
if (mIsMotorEnabled) { if (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity)) {
// Compute J*v for the motor // Compute J*v for the motor
const decimal JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2); const decimal JvMotor = sliderAxisWorld.dot(v1) - sliderAxisWorld.dot(v2);
// Compute the Lagrange multiplier lambda for the motor // Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; const decimal maxMotorImpulse = mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); decimal deltaLambdaMotor = mWorld.mSliderJointsComponents.getInverseMassMatrixMotor(mEntity) * (-JvMotor - mWorld.mSliderJointsComponents.getMotorSpeed(mEntity));
decimal lambdaTemp = mImpulseMotor; decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity);
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, clamp(mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse));
deltaLambdaMotor = mImpulseMotor - lambdaTemp; deltaLambdaMotor = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1 // Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld; const Vector3 linearImpulseBody1 = deltaLambdaMotor * sliderAxisWorld;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2 // Compute the impulse P=J^T * lambda for the motor of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -deltaLambdaMotor * sliderAxisWorld;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -502,33 +533,47 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Vector from body center to the anchor point // Vector from body center to the anchor point
mR1 = q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
mR2 = q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); mWorld.mSliderJointsComponents.setR2(mEntity, q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity));
const Vector3& r1 = mWorld.mSliderJointsComponents.getR1(mEntity);
const Vector3& r2 = mWorld.mSliderJointsComponents.getR2(mEntity);
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1; const Vector3 u = x2 + r2 - x1 - r1;
// Compute the two orthogonal vectors to the slider axis in world-space // Compute the two orthogonal vectors to the slider axis in world-space
mSliderAxisWorld = q1 * mSliderAxisBody1; Vector3 sliderAxisWorld = q1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity);
mSliderAxisWorld.normalize(); sliderAxisWorld.normalize();
mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld);
mN2 = mSliderAxisWorld.cross(mN1); mWorld.mSliderJointsComponents.setN1(mEntity, sliderAxisWorld.getOneUnitOrthogonalVector());
mWorld.mSliderJointsComponents.setN2(mEntity, sliderAxisWorld.cross(n1));
// Check if the limit constraints are violated or not // Check if the limit constraints are violated or not
decimal uDotSliderAxis = u.dot(mSliderAxisWorld); decimal uDotSliderAxis = u.dot(sliderAxisWorld);
decimal lowerLimitError = uDotSliderAxis - mLowerLimit; decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity);
decimal upperLimitError = mUpperLimit - uDotSliderAxis; decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis;
mIsLowerLimitViolated = lowerLimitError <= 0; mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0);
mIsUpperLimitViolated = upperLimitError <= 0; mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0);
// Compute the cross products used in the Jacobians // Compute the cross products used in the Jacobians
mR2CrossN1 = mR2.cross(mN1); mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1));
mR2CrossN2 = mR2.cross(mN2); mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2));
mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld));
const Vector3 r1PlusU = mR1 + u; const Vector3 r1PlusU = r1 + u;
mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1));
mR1PlusUCrossN2 = (r1PlusU).cross(mN2); mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2));
mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); mWorld.mSliderJointsComponents.setR1PlusUCrossSliderAxis(mEntity, r1PlusU.cross(sliderAxisWorld));
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -540,18 +585,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
decimal sumInverseMass = body1MassInverse + body2MassInverse; decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
Vector3 I2R2CrossN1 = i2 * mR2CrossN1; Vector3 I2R2CrossN1 = i2 * r2CrossN1;
Vector3 I2R2CrossN2 = i2 * mR2CrossN2; Vector3 I2R2CrossN2 = i2 * r2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1); r2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
mR2CrossN1.dot(I2R2CrossN2); r2CrossN1.dot(I2R2CrossN2);
const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
mR2CrossN2.dot(I2R2CrossN1); r2CrossN2.dot(I2R2CrossN1);
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2); r2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero(); inverseMassMatrixTranslation.setToZero();
@ -562,15 +607,15 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
} }
// Compute the position error for the 2 translation constraints // Compute the position error for the 2 translation constraints
const Vector2 translationError(u.dot(mN1), u.dot(mN2)); const Vector2 translationError(u.dot(n1), u.dot(n2));
// Compute the Lagrange multiplier lambda for the 2 translation constraints // Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError); Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
mR1PlusUCrossN2 * lambdaTranslation.y; r1PlusUCrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -582,9 +627,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
mR2CrossN2 * lambdaTranslation.y;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -659,29 +703,30 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
if (mIsLimitEnabled) { if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity)) {
if (mIsLowerLimitViolated || mIsUpperLimitViolated) { if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + decimal inverseMassMatrixLimit = body1MassInverse + body2MassInverse +
mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); decimal(1.0) / inverseMassMatrixLimit : decimal(0.0);
mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit);
} }
// If the lower limit is violated // If the lower limit is violated
if (mIsLowerLimitViolated) { if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) {
// Compute the Lagrange multiplier lambda for the lower limit constraint // Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = mInverseMassMatrixLimit * (-lowerLimitError); decimal lambdaLowerLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity) * (-lowerLimitError);
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = -lambdaLowerLimit * sliderAxisWorld;
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -693,8 +738,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; const Vector3 linearImpulseBody2 = lambdaLowerLimit * sliderAxisWorld;
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -707,14 +752,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
} }
// If the upper limit is violated // If the upper limit is violated
if (mIsUpperLimitViolated) { if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
// Compute the Lagrange multiplier lambda for the upper limit constraint // Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = mInverseMassMatrixLimit * (-upperLimitError); decimal lambdaUpperLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity) * (-upperLimitError);
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody1 = lambdaUpperLimit * sliderAxisWorld;
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
@ -726,8 +771,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; const Vector3 linearImpulseBody2 = -lambdaUpperLimit * sliderAxisWorld;
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
@ -753,9 +798,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
*/ */
void SliderJoint::enableLimit(bool isLimitEnabled) { void SliderJoint::enableLimit(bool isLimitEnabled) {
if (isLimitEnabled != mIsLimitEnabled) { bool isEnabled = mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity);
mIsLimitEnabled = isLimitEnabled; if (isLimitEnabled != isEnabled) {
mWorld.mSliderJointsComponents.setIsLimitEnabled(mEntity, isLimitEnabled);
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -769,8 +816,8 @@ void SliderJoint::enableLimit(bool isLimitEnabled) {
*/ */
void SliderJoint::enableMotor(bool isMotorEnabled) { void SliderJoint::enableMotor(bool isMotorEnabled) {
mIsMotorEnabled = isMotorEnabled; mWorld.mSliderJointsComponents.setIsMotorEnabled(mEntity, isMotorEnabled);
mImpulseMotor = 0.0; mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
awakeBodies(); awakeBodies();
@ -805,7 +852,7 @@ decimal SliderJoint::getTranslation() const {
const Vector3 u = anchorBody2 - anchorBody1; const Vector3 u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space // Compute the slider axis in world-space
Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; Vector3 sliderAxisWorld = q1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity);
sliderAxisWorld.normalize(); sliderAxisWorld.normalize();
// Compute and return the translation value // Compute and return the translation value
@ -818,11 +865,11 @@ decimal SliderJoint::getTranslation() const {
*/ */
void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
assert(lowerLimit <= mUpperLimit); assert(lowerLimit <= mWorld.mSliderJointsComponents.getUpperLimit(mEntity));
if (lowerLimit != mLowerLimit) { if (lowerLimit != mWorld.mSliderJointsComponents.getLowerLimit(mEntity)) {
mLowerLimit = lowerLimit; mWorld.mSliderJointsComponents.setLowerLimit(mEntity, lowerLimit);
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -835,11 +882,11 @@ void SliderJoint::setMinTranslationLimit(decimal lowerLimit) {
*/ */
void SliderJoint::setMaxTranslationLimit(decimal upperLimit) { void SliderJoint::setMaxTranslationLimit(decimal upperLimit) {
assert(mLowerLimit <= upperLimit); assert(mWorld.mSliderJointsComponents.getLowerLimit(mEntity) <= upperLimit);
if (upperLimit != mUpperLimit) { if (upperLimit != mWorld.mSliderJointsComponents.getUpperLimit(mEntity)) {
mUpperLimit = upperLimit; mWorld.mSliderJointsComponents.setUpperLimit(mEntity, upperLimit);
// Reset the limits // Reset the limits
resetLimits(); resetLimits();
@ -850,8 +897,8 @@ void SliderJoint::setMaxTranslationLimit(decimal upperLimit) {
void SliderJoint::resetLimits() { void SliderJoint::resetLimits() {
// Reset the accumulated impulses for the limits // Reset the accumulated impulses for the limits
mImpulseLowerLimit = 0.0; mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
mImpulseUpperLimit = 0.0; mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
awakeBodies(); awakeBodies();
@ -863,9 +910,9 @@ void SliderJoint::resetLimits() {
*/ */
void SliderJoint::setMotorSpeed(decimal motorSpeed) { void SliderJoint::setMotorSpeed(decimal motorSpeed) {
if (motorSpeed != mMotorSpeed) { if (motorSpeed != mWorld.mSliderJointsComponents.getMotorSpeed(mEntity)) {
mMotorSpeed = motorSpeed; mWorld.mSliderJointsComponents.setMotorSpeed(mEntity, motorSpeed);
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
awakeBodies(); awakeBodies();
@ -878,23 +925,82 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
*/ */
void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
if (maxMotorForce != mMaxMotorForce) { const decimal maxForce = mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity);
assert(mMaxMotorForce >= decimal(0.0)); if (maxMotorForce != maxForce) {
mMaxMotorForce = maxMotorForce;
assert(maxForce >= decimal(0.0));
mWorld.mSliderJointsComponents.setMaxMotorForce(mEntity, maxMotorForce);
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
awakeBodies(); awakeBodies();
} }
} }
// Return the intensity of the current force applied for the joint motor
/**
* @param timeStep Time step (in seconds)
* @return The current force of the joint motor (in Newton x meters)
*/
decimal SliderJoint::getMotorForce(decimal timeStep) const {
return mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) / timeStep;
}
// Return true if the limits or the joint are enabled
/**
* @return True if the joint limits are enabled
*/
bool SliderJoint::isLimitEnabled() const {
return mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity);
}
// Return true if the motor of the joint is enabled
/**
* @return True if the joint motor is enabled
*/
bool SliderJoint::isMotorEnabled() const {
return mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity);
}
// Return the minimum translation limit
/**
* @return The minimum translation limit of the joint (in meters)
*/
decimal SliderJoint::getMinTranslationLimit() const {
return mWorld.mSliderJointsComponents.getLowerLimit(mEntity);
}
// Return the motor speed
/**
* @return The current motor speed of the joint (in meters per second)
*/
decimal SliderJoint::getMotorSpeed() const {
return mWorld.mSliderJointsComponents.getMotorSpeed(mEntity);
}
// Return the maximum motor force
/**
* @return The maximum force of the joint motor (in Newton x meters)
*/
decimal SliderJoint::getMaxMotorForce() const {
return mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity);
}
// Return the maximum translation limit
/**
* @return The maximum translation limit of the joint (in meters)
*/
decimal SliderJoint::getMaxTranslationLimit() const {
return mWorld.mSliderJointsComponents.getUpperLimit(mEntity);
}
// Return a string representation // Return a string representation
std::string SliderJoint::to_string() const { std::string SliderJoint::to_string() const {
return "SliderJoint{ lowerLimit=" + std::to_string(mLowerLimit) + ", upperLimit=" + std::to_string(mUpperLimit) + return "SliderJoint{ lowerLimit=" + std::to_string(mWorld.mSliderJointsComponents.getLowerLimit(mEntity)) + ", upperLimit=" + std::to_string(mWorld.mSliderJointsComponents.getUpperLimit(mEntity)) +
"localAnchorPointBody1=" + mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + ", localAnchorPointBody2=" + "localAnchorPointBody1=" + mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + ", localAnchorPointBody2=" +
mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", sliderAxisBody1=" + mSliderAxisBody1.to_string() + mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", sliderAxisBody1=" + mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity).to_string() +
", initOrientationDifferenceInv=" + ", initOrientationDifferenceInv=" +
mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) + mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + ", motorSpeed=" + std::to_string(getMotorSpeed()) +
", maxMotorForce=" + std::to_string(mMaxMotorForce) + ", isLimitEnabled=" + ", maxMotorForce=" + std::to_string(getMaxMotorForce()) + ", isLimitEnabled=" +
(mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}"; (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity) ? "true" : "false") + ", isMotorEnabled=" + (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity) ? "true" : "false") + "}";
} }

138
src/constraint/SliderJoint.h
View File

@ -150,87 +150,6 @@ class SliderJoint : public Joint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Vector r1 in world-space coordinates
Vector3 mR1;
/// Vector r2 in world-space coordinates
Vector3 mR2;
/// Slider axis (in local-space coordinates of body 1)
Vector3 mSliderAxisBody1;
/// First vector orthogonal to the slider axis local-space of body 1
Vector3 mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3 mN2;
/// Cross product of r2 and n1
Vector3 mR2CrossN1;
/// Cross product of r2 and n2
Vector3 mR2CrossN2;
/// Cross product of r2 and the slider axis
Vector3 mR2CrossSliderAxis;
/// Cross product of vector (r1 + u) and n1
Vector3 mR1PlusUCrossN1;
/// Cross product of vector (r1 + u) and n2
Vector3 mR1PlusUCrossN2;
/// Cross product of vector (r1 + u) and the slider axis
Vector3 mR1PlusUCrossSliderAxis;
/// Bias of the lower limit constraint
decimal mBLowerLimit;
/// Bias of the upper limit constraint
decimal mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
decimal mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor
decimal mInverseMassMatrixMotor;
/// Accumulated impulse for the lower limit constraint
decimal mImpulseLowerLimit;
/// Accumulated impulse for the upper limit constraint
decimal mImpulseUpperLimit;
/// Accumulated impulse for the motor
decimal mImpulseMotor;
/// True if the slider limits are enabled
bool mIsLimitEnabled;
/// True if the motor of the joint in enabled
bool mIsMotorEnabled;
/// Slider axis in world-space coordinates
Vector3 mSliderAxisWorld;
/// Lower limit (minimum translation distance)
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 m/s)
decimal mMotorSpeed;
/// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
decimal mMaxMotorForce;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Reset the limits /// Reset the limits
@ -313,63 +232,6 @@ class SliderJoint : public Joint {
virtual std::string to_string() const override; virtual std::string to_string() const override;
}; };
// Return true if the limits or the joint are enabled
/**
* @return True if the joint limits are enabled
*/
inline bool SliderJoint::isLimitEnabled() const {
return mIsLimitEnabled;
}
// Return true if the motor of the joint is enabled
/**
* @return True if the joint motor is enabled
*/
inline bool SliderJoint::isMotorEnabled() const {
return mIsMotorEnabled;
}
// Return the minimum translation limit
/**
* @return The minimum translation limit of the joint (in meters)
*/
inline decimal SliderJoint::getMinTranslationLimit() const {
return mLowerLimit;
}
// Return the maximum translation limit
/**
* @return The maximum translation limit of the joint (in meters)
*/
inline decimal SliderJoint::getMaxTranslationLimit() const {
return mUpperLimit;
}
// Return the motor speed
/**
* @return The current motor speed of the joint (in meters per second)
*/
inline decimal SliderJoint::getMotorSpeed() const {
return mMotorSpeed;
}
// Return the maximum motor force
/**
* @return The maximum force of the joint motor (in Newton x meters)
*/
inline decimal SliderJoint::getMaxMotorForce() const {
return mMaxMotorForce;
}
// Return the intensity of the current force applied for the joint motor
/**
* @param timeStep Time step (in seconds)
* @return The current force of the joint motor (in Newton x meters)
*/
inline decimal SliderJoint::getMotorForce(decimal timeStep) const {
return mImpulseMotor / timeStep;
}
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
inline size_t SliderJoint::getSizeInBytes() const { inline size_t SliderJoint::getSizeInBytes() const {
return sizeof(SliderJoint); return sizeof(SliderJoint);

10
src/engine/DynamicsWorld.cpp
View File

@ -348,13 +348,15 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Slider joint // Slider joint
case JointType::SLIDERJOINT: case JointType::SLIDERJOINT:
{ {
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(jointInfo);
// Create a SliderJoint component // Create a SliderJoint component
SliderJointComponents::SliderJointComponent sliderJointComponent; SliderJointComponents::SliderJointComponent sliderJointComponent(info.isLimitEnabled, info.isMotorEnabled,
info.minTranslationLimit, info.maxTranslationLimit,
info.motorSpeed, info.maxMotorForce);
mSliderJointsComponents.addComponent(entity, isJointDisabled, sliderJointComponent); mSliderJointsComponents.addComponent(entity, isJointDisabled, sliderJointComponent);
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint));
sizeof(SliderJoint));
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(jointInfo);
SliderJoint* joint = new (allocatedMemory) SliderJoint(entity, *this, info); SliderJoint* joint = new (allocatedMemory) SliderJoint(entity, *this, info);
newJoint = joint; newJoint = joint;