diff --git a/src/components/HingeJointComponents.cpp b/src/components/HingeJointComponents.cpp index 6a71ba9c..253e56dd 100644 --- a/src/components/HingeJointComponents.cpp +++ b/src/components/HingeJointComponents.cpp @@ -210,6 +210,7 @@ void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, con new (mC2CrossA1 + index) Vector3(0, 0, 0); mImpulseLowerLimit[index] = decimal(0.0); mImpulseUpperLimit[index] = decimal(0.0); + mImpulseMotor[index] = decimal(0.0); mInverseMassMatrixLimitMotor[index] = decimal(0.0); mInverseMassMatrixMotor[index] = decimal(0.0); mBLowerLimit[index] = decimal(0.0); diff --git a/src/components/SliderJointComponents.cpp b/src/components/SliderJointComponents.cpp index d7bdc751..c1e1ce4c 100644 --- a/src/components/SliderJointComponents.cpp +++ b/src/components/SliderJointComponents.cpp @@ -35,15 +35,17 @@ using namespace reactphysics3d; // Constructor SliderJointComponents::SliderJointComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(SliderJoint*) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector2) + sizeof(Vector3) + sizeof(Matrix2x2) + sizeof(Matrix3x3) + - sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion)/* + + sizeof(Vector2) + sizeof(Vector3) + sizeof(Quaternion) + 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(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(INIT_NB_ALLOCATED_COMPONENTS); @@ -75,17 +77,17 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { Vector2* newBiasTranslation = reinterpret_cast(newInverseMassMatrixRotation + nbComponentsToAllocate); Vector3* newBiasRotation = reinterpret_cast(newBiasTranslation + nbComponentsToAllocate); Quaternion* newInitOrientationDifferenceInv = reinterpret_cast(newBiasRotation + nbComponentsToAllocate); - /* - Vector3* newHingeLocalAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); - Vector3* newHingeLocalAxisBody2 = reinterpret_cast(newHingeLocalAxisBody1 + nbComponentsToAllocate); - Vector3* newA1 = reinterpret_cast(newHingeLocalAxisBody2 + nbComponentsToAllocate); - Vector3* newB2CrossA1 = reinterpret_cast(newA1 + nbComponentsToAllocate); - Vector3* newC2CrossA1 = reinterpret_cast(newB2CrossA1 + nbComponentsToAllocate); - decimal* newImpulseLowerLimit = reinterpret_cast(newC2CrossA1 + nbComponentsToAllocate); + Vector3* newSliderAxisBody1 = reinterpret_cast(newInitOrientationDifferenceInv + nbComponentsToAllocate); + Vector3* newSliderAxisWorld = reinterpret_cast(newSliderAxisBody1 + nbComponentsToAllocate); + Vector3* newR1 = reinterpret_cast(newSliderAxisWorld + nbComponentsToAllocate); + Vector3* newR2 = reinterpret_cast(newR1 + nbComponentsToAllocate); + Vector3* newN1 = reinterpret_cast(newR2 + nbComponentsToAllocate); + Vector3* newN2 = reinterpret_cast(newN1 + nbComponentsToAllocate); + decimal* newImpulseLowerLimit = reinterpret_cast(newN2 + nbComponentsToAllocate); decimal* newImpulseUpperLimit = reinterpret_cast(newImpulseLowerLimit + nbComponentsToAllocate); decimal* newImpulseMotor = reinterpret_cast(newImpulseUpperLimit + nbComponentsToAllocate); - decimal* newInverseMassMatrixLimitMotor = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); - decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimitMotor + nbComponentsToAllocate); + decimal* newInverseMassMatrixLimit = reinterpret_cast(newImpulseMotor + nbComponentsToAllocate); + decimal* newInverseMassMatrixMotor = reinterpret_cast(newInverseMassMatrixLimit + nbComponentsToAllocate); decimal* newBLowerLimit = reinterpret_cast(newInverseMassMatrixMotor + nbComponentsToAllocate); decimal* newBUpperLimit = reinterpret_cast(newBLowerLimit + nbComponentsToAllocate); bool* newIsLimitEnabled = reinterpret_cast(newBUpperLimit + nbComponentsToAllocate); @@ -95,8 +97,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { bool* newIsLowerLimitViolated = reinterpret_cast(newUpperLimit + nbComponentsToAllocate); bool* newIsUpperLimitViolated = reinterpret_cast(newIsLowerLimitViolated + nbComponentsToAllocate); decimal* newMotorSpeed = reinterpret_cast(newIsUpperLimitViolated + nbComponentsToAllocate); - decimal* newMaxMotorTorque = reinterpret_cast(newMotorSpeed + nbComponentsToAllocate); - */ + decimal* newMaxMotorForce = reinterpret_cast(newMotorSpeed + nbComponentsToAllocate); + Vector3* newR2CrossN1 = reinterpret_cast(newMaxMotorForce + nbComponentsToAllocate); + Vector3* newR2CrossN2 = reinterpret_cast(newR2CrossN1 + nbComponentsToAllocate); + Vector3* newR2CrossSliderAxis = reinterpret_cast(newR2CrossN2 + nbComponentsToAllocate); + Vector3* newR1PlusUCrossN1 = reinterpret_cast(newR2CrossSliderAxis + nbComponentsToAllocate); + Vector3* newR1PlusUCrossN2 = reinterpret_cast(newR1PlusUCrossN1 + nbComponentsToAllocate); + Vector3* newR1PlusUCrossSliderAxis = reinterpret_cast(newR1PlusUCrossN2 + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -115,16 +122,16 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector2)); memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3)); memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion)); - /* - memcpy(newHingeLocalAxisBody1, mHingeLocalAxisBody1, mNbComponents * sizeof(Vector3)); - memcpy(newHingeLocalAxisBody2, mHingeLocalAxisBody2, mNbComponents * sizeof(Vector3)); - memcpy(newA1, mA1, mNbComponents * sizeof(Vector3)); - memcpy(newB2CrossA1, mB2CrossA1, mNbComponents * sizeof(Vector3)); - memcpy(newC2CrossA1, mC2CrossA1, mNbComponents * sizeof(Vector3)); + memcpy(newSliderAxisBody1, mSliderAxisBody1, mNbComponents * sizeof(Vector3)); + memcpy(newSliderAxisWorld, mSliderAxisWorld, mNbComponents * sizeof(Vector3)); + memcpy(newR1, mR1, mNbComponents * sizeof(Vector3)); + memcpy(newR2, mR2, mNbComponents * sizeof(Vector3)); + memcpy(newN1, mN1, mNbComponents * sizeof(Vector3)); + memcpy(newN2, mN2, mNbComponents * sizeof(Vector3)); memcpy(newImpulseLowerLimit, mImpulseLowerLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseUpperLimit, mImpulseUpperLimit, mNbComponents * sizeof(decimal)); memcpy(newImpulseMotor, mImpulseMotor, mNbComponents * sizeof(decimal)); - memcpy(newInverseMassMatrixLimitMotor, mInverseMassMatrixLimitMotor, mNbComponents * sizeof(decimal)); + memcpy(newInverseMassMatrixLimit, mInverseMassMatrixLimit, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixMotor, mInverseMassMatrixMotor, mNbComponents * sizeof(decimal)); memcpy(newBLowerLimit, mBLowerLimit, 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(newIsUpperLimitViolated, mIsUpperLimitViolated, mNbComponents * sizeof(bool)); 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 mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -157,16 +169,16 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { mBiasTranslation = newBiasTranslation; mBiasRotation = newBiasRotation; mInitOrientationDifferenceInv = newInitOrientationDifferenceInv; - /* - mHingeLocalAxisBody1 = newHingeLocalAxisBody1; - mHingeLocalAxisBody2 = newHingeLocalAxisBody2; - mA1 = newA1; - mB2CrossA1 = newB2CrossA1; - mC2CrossA1 = newC2CrossA1; + mSliderAxisBody1 = newSliderAxisBody1; + mSliderAxisWorld = newSliderAxisWorld; + mR1 = newR1; + mR2 = newR2; + mN1 = newN1; + mN2 = newN2; mImpulseLowerLimit = newImpulseLowerLimit; mImpulseUpperLimit = newImpulseUpperLimit; mImpulseMotor = newImpulseMotor; - mInverseMassMatrixLimitMotor = newInverseMassMatrixLimitMotor; + mInverseMassMatrixLimit = newInverseMassMatrixLimit; mInverseMassMatrixMotor = newInverseMassMatrixMotor; mBLowerLimit = newBLowerLimit; mBUpperLimit = newBUpperLimit; @@ -177,8 +189,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) { mIsLowerLimitViolated = newIsLowerLimitViolated; mIsUpperLimitViolated = newIsUpperLimitViolated; mMotorSpeed = newMotorSpeed; - mMaxMotorTorque = newMaxMotorTorque; - */ + mMaxMotorForce = newMaxMotorForce; + mR2CrossN1 = newR2CrossN1; + mR2CrossN2 = newR2CrossN2; + mR2CrossSliderAxis = newR2CrossSliderAxis; + mR1PlusUCrossN1 = newR1PlusUCrossN1; + mR1PlusUCrossN2 = newR1PlusUCrossN2; + mR1PlusUCrossSliderAxis = newR1PlusUCrossSliderAxis; } // Add a component @@ -201,15 +218,16 @@ void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, co new (mBiasTranslation + index) Vector2(0, 0); new (mBiasRotation + index) Vector3(0, 0, 0); new (mInitOrientationDifferenceInv + index) Quaternion(0, 0, 0, 0); - /* - new (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0); - new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0); - new (mA1 + index) Vector3(0, 0, 0); - new (mB2CrossA1 + index) Vector3(0, 0, 0); - new (mC2CrossA1 + index) Vector3(0, 0, 0); + new (mSliderAxisBody1 + index) Vector3(0, 0, 0); + new (mSliderAxisWorld + index) Vector3(0, 0, 0); + new (mR1 + index) Vector3(0, 0, 0); + new (mR2 + index) Vector3(0, 0, 0); + new (mN1 + index) Vector3(0, 0, 0); + new (mN2 + index) Vector3(0, 0, 0); mImpulseLowerLimit[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); mBLowerLimit[index] = decimal(0.0); mBUpperLimit[index] = decimal(0.0); @@ -220,8 +238,13 @@ void SliderJointComponents::addComponent(Entity jointEntity, bool isSleeping, co mIsLowerLimitViolated[index] = false; mIsUpperLimitViolated[index] = false; 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 mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -252,16 +275,16 @@ void SliderJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInd new (mBiasTranslation + destIndex) Vector2(mBiasTranslation[srcIndex]); new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]); new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[srcIndex]); - /* - new (mHingeLocalAxisBody1 + destIndex) Vector3(mHingeLocalAxisBody1[srcIndex]); - new (mHingeLocalAxisBody2 + destIndex) Vector3(mHingeLocalAxisBody2[srcIndex]); - new (mA1 + destIndex) Vector3(mA1[srcIndex]); - new (mB2CrossA1 + destIndex) Vector3(mB2CrossA1[srcIndex]); - new (mC2CrossA1 + destIndex) Vector3(mC2CrossA1[srcIndex]); + new (mSliderAxisBody1 + destIndex) Vector3(mSliderAxisBody1[srcIndex]); + new (mSliderAxisWorld + destIndex) Vector3(mSliderAxisWorld[srcIndex]); + new (mR1 + destIndex) Vector3(mR1[srcIndex]); + new (mR2 + destIndex) Vector3(mR2[srcIndex]); + new (mN1 + destIndex) Vector3(mN1[srcIndex]); + new (mN2 + destIndex) Vector3(mN2[srcIndex]); mImpulseLowerLimit[destIndex] = mImpulseLowerLimit[srcIndex]; mImpulseUpperLimit[destIndex] = mImpulseUpperLimit[srcIndex]; mImpulseMotor[destIndex] = mImpulseMotor[srcIndex]; - mInverseMassMatrixLimitMotor[destIndex] = mInverseMassMatrixLimitMotor[srcIndex]; + mInverseMassMatrixLimit[destIndex] = mInverseMassMatrixLimit[srcIndex]; mInverseMassMatrixMotor[destIndex] = mInverseMassMatrixMotor[srcIndex]; mBLowerLimit[destIndex] = mBLowerLimit[srcIndex]; mBUpperLimit[destIndex] = mBUpperLimit[srcIndex]; @@ -272,8 +295,13 @@ void SliderJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInd mIsLowerLimitViolated[destIndex] = mIsLowerLimitViolated[srcIndex]; mIsUpperLimitViolated[destIndex] = mIsUpperLimitViolated[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 destroyComponent(srcIndex); @@ -303,16 +331,16 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { Vector2 biasTranslation1(mBiasTranslation[index1]); Vector3 biasRotation1(mBiasRotation[index1]); Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[index1]); - /* - Vector3 hingeLocalAxisBody1(mHingeLocalAxisBody1[index1]); - Vector3 hingeLocalAxisBody2(mHingeLocalAxisBody2[index1]); - Vector3 a1(mA1[index1]); - Vector3 b2CrossA1(mB2CrossA1[index1]); - Vector3 c2CrossA1(mC2CrossA1[index1]); + Vector3 sliderAxisBody1(mSliderAxisBody1[index1]); + Vector3 sliderAxisWorld(mSliderAxisWorld[index1]); + Vector3 r1(mR1[index1]); + Vector3 r2(mR2[index1]); + Vector3 n1(mN1[index1]); + Vector3 n2(mN2[index1]); decimal impulseLowerLimit(mImpulseLowerLimit[index1]); decimal impulseUpperLimit(mImpulseUpperLimit[index1]); decimal impulseMotor(mImpulseMotor[index1]); - decimal inverseMassMatrixLimitMotor(mInverseMassMatrixLimitMotor[index1]); + decimal inverseMassMatrixLimit(mInverseMassMatrixLimit[index1]); decimal inverseMassMatrixMotor(mInverseMassMatrixMotor[index1]); decimal bLowerLimit(mBLowerLimit[index1]); decimal bUpperLimit(mUpperLimit[index1]); @@ -323,8 +351,13 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { bool isLowerLimitViolated(mIsLowerLimitViolated[index1]); bool isUpperLimitViolated(mIsUpperLimitViolated[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 destroyComponent(index1); @@ -345,16 +378,16 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { new (mBiasTranslation + index2) Vector2(biasTranslation1); new (mBiasRotation + index2) Vector3(biasRotation1); new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1); - /* - new (mHingeLocalAxisBody1 + index2) Vector3(hingeLocalAxisBody1); - new (mHingeLocalAxisBody2 + index2) Vector3(hingeLocalAxisBody2); - new (mA1 + index2) Vector3(a1); - new (mB2CrossA1 + index2) Vector3(b2CrossA1); - new (mC2CrossA1 + index2) Vector3(c2CrossA1); + new (mSliderAxisBody1 + index2) Vector3(sliderAxisBody1); + new (mSliderAxisWorld + index2) Vector3(sliderAxisWorld); + new (mR1 + index2) Vector3(r1); + new (mR2 + index2) Vector3(r2); + new (mN1 + index2) Vector3(n1); + new (mN2 + index2) Vector3(n2); mImpulseLowerLimit[index2] = impulseLowerLimit; mImpulseUpperLimit[index2] = impulseUpperLimit; mImpulseMotor[index2] = impulseMotor; - mInverseMassMatrixLimitMotor[index2] = inverseMassMatrixLimitMotor; + mInverseMassMatrixLimit[index2] = inverseMassMatrixLimit; mInverseMassMatrixMotor[index2] = inverseMassMatrixMotor; mBLowerLimit[index2] = bLowerLimit; mBUpperLimit[index2] = bUpperLimit; @@ -365,8 +398,13 @@ void SliderJointComponents::swapComponents(uint32 index1, uint32 index2) { mIsLowerLimitViolated[index2] = isLowerLimitViolated; mIsUpperLimitViolated[index2] = isUpperLimitViolated; 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 mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); @@ -398,11 +436,16 @@ void SliderJointComponents::destroyComponent(uint32 index) { mBiasTranslation[index].~Vector2(); mBiasRotation[index].~Vector3(); mInitOrientationDifferenceInv[index].~Quaternion(); - /* - mHingeLocalAxisBody1[index].~Vector3(); - mHingeLocalAxisBody2[index].~Vector3(); - mA1[index].~Vector3(); - mB2CrossA1[index].~Vector3(); - mC2CrossA1[index].~Vector3(); - */ + mSliderAxisBody1[index].~Vector3(); + mSliderAxisWorld[index].~Vector3(); + mR1[index].~Vector3(); + mR2[index].~Vector3(); + mN1[index].~Vector3(); + mN2[index].~Vector3(); + mR2CrossN1[index].~Vector3(); + mR2CrossN2[index].~Vector3(); + mR2CrossSliderAxis[index].~Vector3(); + mR1PlusUCrossN1[index].~Vector3(); + mR1PlusUCrossN2[index].~Vector3(); + mR1PlusUCrossSliderAxis[index].~Vector3(); } diff --git a/src/components/SliderJointComponents.h b/src/components/SliderJointComponents.h index 848330a2..9b206b04 100644 --- a/src/components/SliderJointComponents.h +++ b/src/components/SliderJointComponents.h @@ -92,21 +92,23 @@ class SliderJointComponents : public Components { /// Inverse of the initial orientation difference between the two bodies Quaternion* mInitOrientationDifferenceInv; - /* - /// Hinge rotation axis (in local-space coordinates of body 1) - Vector3* mHingeLocalAxisBody1; + /// Slider axis (in local-space coordinates of body 1) + Vector3* mSliderAxisBody1; - /// Hinge rotation axis (in local-space coordiantes of body 2) - Vector3* mHingeLocalAxisBody2; + /// Slider axis in world-space coordinates + Vector3* mSliderAxisWorld; - /// Hinge rotation axis (in world-space coordinates) computed from body 1 - Vector3* mA1; + /// Vector r1 in world-space coordinates + Vector3* mR1; - /// Cross product of vector b2 and a1 - Vector3* mB2CrossA1; + /// Vector r2 in world-space coordinates + Vector3* mR2; - /// Cross product of vector c2 and a1; - Vector3* mC2CrossA1; + /// 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; /// Accumulated impulse for the lower limit constraint decimal* mImpulseLowerLimit; @@ -117,8 +119,8 @@ class SliderJointComponents : public Components { /// Accumulated impulse for the motor constraint; decimal* mImpulseMotor; - /// Inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) - decimal* mInverseMassMatrixLimitMotor; + /// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix) + decimal* mInverseMassMatrixLimit; /// Inverse of mass matrix K=JM^-1J^t for the motor decimal* mInverseMassMatrixMotor; @@ -150,10 +152,26 @@ class SliderJointComponents : public Components { /// Motor speed (in rad/s) decimal* mMotorSpeed; - /// Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - decimal* mMaxMotorTorque; + /// Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + 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 -------------------- // @@ -179,18 +197,13 @@ class SliderJointComponents : public Components { decimal lowerLimit; decimal upperLimit; decimal motorSpeed; - decimal maxMotorTorque; - - // TODO : Delete this - SliderJointComponent() { - - } + decimal maxMotorForce; /// Constructor SliderJointComponent(bool isLimitEnabled, bool isMotorEnabled, decimal lowerLimit, decimal upperLimit, - decimal motorSpeed, decimal maxMotorTorque) - : isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit), - motorSpeed(motorSpeed), maxMotorTorque(maxMotorTorque) { + decimal motorSpeed, decimal maxMotorForce) + :isLimitEnabled(isLimitEnabled), isMotorEnabled(isMotorEnabled), lowerLimit(lowerLimit), upperLimit(upperLimit), + motorSpeed(motorSpeed), maxMotorForce(maxMotorForce) { } }; @@ -278,36 +291,41 @@ class SliderJointComponents : public Components { /// Set the rotation impulse void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv); - /* - /// Return the hinge rotation axis (in local-space coordinates of body 1) - Vector3& getHingeLocalAxisBody1(Entity jointEntity); + /// Return the slider axis (in local-space coordinates of body 1) + Vector3& getSliderAxisBody1(Entity jointEntity); - /// Set the hinge rotation axis (in local-space coordinates of body 1) - void setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1); + /// Set the slider axis (in local-space coordinates of body 1) + void setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1); - /// Return the hinge rotation axis (in local-space coordiantes of body 2) - Vector3& getHingeLocalAxisBody2(Entity jointEntity); + /// Retunr the slider axis in world-space coordinates + Vector3& getSliderAxisWorld(Entity jointEntity); - /// Set the hinge rotation axis (in local-space coordiantes of body 2) - void setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2); + /// Set the slider axis in world-space coordinates + void setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld); - /// Return the hinge rotation axis (in world-space coordinates) computed from body 1 - Vector3& getA1(Entity jointEntity); + /// Return the vector r1 in world-space coordinates + Vector3& getR1(Entity jointEntity); - /// Set the hinge rotation axis (in world-space coordinates) computed from body 1 - void setA1(Entity jointEntity, const Vector3& a1); + /// Set the vector r1 in world-space coordinates + void setR1(Entity jointEntity, const Vector3& r1); - /// Return the cross product of vector b2 and a1 - Vector3& getB2CrossA1(Entity jointEntity); + /// Return the vector r2 in world-space coordinates + Vector3& getR2(Entity jointEntity); - /// Set the cross product of vector b2 and a1 - void setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1); + /// Set the vector r2 in world-space coordinates + void setR2(Entity jointEntity, const Vector3& r2); - /// Return the cross product of vector c2 and a1; - Vector3& getC2CrossA1(Entity jointEntity); + /// Return the first vector orthogonal to the slider axis local-space of body 1 + Vector3& getN1(Entity jointEntity); - /// Set the cross product of vector c2 and a1; - void setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1); + /// Set the first vector orthogonal to the slider axis local-space of body 1 + 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 decimal getImpulseLowerLimit(Entity jointEntity) const; @@ -327,11 +345,11 @@ class SliderJointComponents : public Components { /// Set the accumulated impulse for the motor constraint; void setImpulseMotor(Entity jointEntity, decimal impulseMotor); - /// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) - decimal getInverseMassMatrixLimitMotor(Entity jointEntity) const; + /// Return the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) + decimal getInverseMassMatrixLimit(Entity jointEntity) const; - /// Set the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) - void setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor); + /// Set the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) + void setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor); /// Return the inverse of mass matrix K=JM^-1J^t for the motor decimal getInverseMassMatrixMotor(Entity jointEntity); @@ -393,12 +411,47 @@ class SliderJointComponents : public Components { /// Set the motor speed (in rad/s) void setMotorSpeed(Entity jointEntity, decimal motorSpeed); - /// Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - decimal getMaxMotorTorque(Entity jointEntity) const; + /// Return the maximum motor force (in Newtons) that can be applied to reach to desired motor speed + decimal getMaxMotorForce(Entity jointEntity) const; - /// Set the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - void setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque); - */ + /// Set the maximum motor force (in Newtons) that can be applied to reach to desired motor speed + 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 -------------------- // @@ -573,87 +626,99 @@ inline void SliderJointComponents::setInitOrientationDifferenceInv(Entity jointE mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv; } -/* -// Return the hinge rotation axis (in local-space coordinates of body 1) -inline Vector3& HingeJointComponents::getHingeLocalAxisBody1(Entity jointEntity) { +// Return the slider axis (in local-space coordinates of body 1) +inline Vector3& SliderJointComponents::getSliderAxisBody1(Entity 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) -inline void HingeJointComponents::setHingeLocalAxisBody1(Entity jointEntity, const Vector3& hingeLocalAxisBody1) { +// Set the slider axis (in local-space coordinates of body 1) +inline void SliderJointComponents::setSliderAxisBody1(Entity jointEntity, const Vector3& sliderAxisBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mHingeLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody1; + mSliderAxisBody1[mMapEntityToComponentIndex[jointEntity]] = sliderAxisBody1; } -// Return the hinge rotation axis (in local-space coordiantes of body 2) -inline Vector3& HingeJointComponents::getHingeLocalAxisBody2(Entity jointEntity) { +// Retunr the slider axis in world-space coordinates +inline Vector3& SliderJointComponents::getSliderAxisWorld(Entity 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) -inline void HingeJointComponents::setHingeLocalAxisBody2(Entity jointEntity, const Vector3& hingeLocalAxisBody2) { +// Set the slider axis in world-space coordinates +inline void SliderJointComponents::setSliderAxisWorld(Entity jointEntity, const Vector3& sliderAxisWorld) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mHingeLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = hingeLocalAxisBody2; + mSliderAxisWorld[mMapEntityToComponentIndex[jointEntity]] = sliderAxisWorld; } - -// Return the hinge rotation axis (in world-space coordinates) computed from body 1 -inline Vector3& HingeJointComponents::getA1(Entity jointEntity) { +// Return the vector r1 in world-space coordinates +inline Vector3& SliderJointComponents::getR1(Entity 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 -inline void HingeJointComponents::setA1(Entity jointEntity, const Vector3& a1) { +// Set the vector r1 in world-space coordinates +inline void SliderJointComponents::setR1(Entity jointEntity, const Vector3& r1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mA1[mMapEntityToComponentIndex[jointEntity]] = a1; + mR1[mMapEntityToComponentIndex[jointEntity]] = r1; } -// Return the cross product of vector b2 and a1 -inline Vector3& HingeJointComponents::getB2CrossA1(Entity jointEntity) { +// Return the vector r2 in world-space coordinates +inline Vector3& SliderJointComponents::getR2(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - return mB2CrossA1[mMapEntityToComponentIndex[jointEntity]]; + return mR2[mMapEntityToComponentIndex[jointEntity]]; } -// Set the cross product of vector b2 and a1 -inline void HingeJointComponents::setB2CrossA1(Entity jointEntity, const Vector3& b2CrossA1) { +// Set the vector r2 in world-space coordinates +inline void SliderJointComponents::setR2(Entity jointEntity, const Vector3& r2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mB2CrossA1[mMapEntityToComponentIndex[jointEntity]] = b2CrossA1; + mR2[mMapEntityToComponentIndex[jointEntity]] = r2; } -// Return the cross product of vector c2 and a1; -inline Vector3& HingeJointComponents::getC2CrossA1(Entity jointEntity) { +// Return the first vector orthogonal to the slider axis local-space of body 1 +inline Vector3& SliderJointComponents::getN1(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - return mC2CrossA1[mMapEntityToComponentIndex[jointEntity]]; + return mN1[mMapEntityToComponentIndex[jointEntity]]; } -// Set the cross product of vector c2 and a1; -inline void HingeJointComponents::setC2CrossA1(Entity jointEntity, const Vector3& c2CrossA1) { +// Set the first vector orthogonal to the slider axis local-space of body 1 +inline void SliderJointComponents::setN1(Entity jointEntity, const Vector3& n1) { 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 -inline decimal HingeJointComponents::getImpulseLowerLimit(Entity jointEntity) const { +inline decimal SliderJointComponents::getImpulseLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the accumulated impulse for the lower limit constraint -inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { +inline void SliderJointComponents::setImpulseLowerLimit(Entity jointEntity, decimal impulseLowerLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mImpulseLowerLimit[mMapEntityToComponentIndex[jointEntity]] = impulseLowerLimit; @@ -661,14 +726,14 @@ inline void HingeJointComponents::setImpulseLowerLimit(Entity jointEntity, decim // 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)); return mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // 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)); mImpulseUpperLimit[mMapEntityToComponentIndex[jointEntity]] = impulseUpperLimit; @@ -676,187 +741,270 @@ inline void HingeJointComponents::setImpulseUpperLimit(Entity jointEntity, decim // 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)); return mImpulseMotor[mMapEntityToComponentIndex[jointEntity]]; } // 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)); mImpulseMotor[mMapEntityToComponentIndex[jointEntity]] = impulseMotor; } -// Return the inverse of mass matrix K=JM^-1J^t for the limits and motor constraints (1x1 matrix) -inline decimal HingeJointComponents::getInverseMassMatrixLimitMotor(Entity jointEntity) const { +// Return the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) +inline decimal SliderJointComponents::getInverseMassMatrixLimit(Entity jointEntity) const { 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) -inline void HingeJointComponents::setInverseMassMatrixLimitMotor(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { +// Set the inverse of mass matrix K=JM^-1J^t for the limits (1x1 matrix) +inline void SliderJointComponents::setInverseMassMatrixLimit(Entity jointEntity, decimal inverseMassMatrixLimitMotor) { 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 -inline decimal HingeJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { +inline decimal SliderJointComponents::getInverseMassMatrixMotor(Entity jointEntity) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]]; } // Return the inverse of mass matrix K=JM^-1J^t for the motor -inline void HingeJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { +inline void SliderJointComponents::setInverseMassMatrixMotor(Entity jointEntity, decimal inverseMassMatrixMotor) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mInverseMassMatrixMotor[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrixMotor; } // Return the bias of the lower limit constraint -inline decimal HingeJointComponents::getBLowerLimit(Entity jointEntity) const { +inline decimal SliderJointComponents::getBLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the lower limit constraint -inline void HingeJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { +inline void SliderJointComponents::setBLowerLimit(Entity jointEntity, decimal bLowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBLowerLimit[mMapEntityToComponentIndex[jointEntity]] = bLowerLimit; } // Return the bias of the upper limit constraint -inline decimal HingeJointComponents::getBUpperLimit(Entity jointEntity) const { +inline decimal SliderJointComponents::getBUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mBUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the bias of the upper limit constraint -inline void HingeJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { +inline void SliderJointComponents::setBUpperLimit(Entity jointEntity, decimal bUpperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mBUpperLimit[mMapEntityToComponentIndex[jointEntity]] = bUpperLimit; } // Return true if the joint limits are enabled -inline bool HingeJointComponents::getIsLimitEnabled(Entity jointEntity) const { +inline bool SliderJointComponents::getIsLimitEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the joint limits are enabled -inline void HingeJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { +inline void SliderJointComponents::setIsLimitEnabled(Entity jointEntity, bool isLimitEnabled) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled; } // Return true if the motor of the joint in enabled -inline bool HingeJointComponents::getIsMotorEnabled(Entity jointEntity) const { +inline bool SliderJointComponents::getIsMotorEnabled(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the motor of the joint in enabled -inline void HingeJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { +inline void SliderJointComponents::setIsMotorEnabled(Entity jointEntity, bool isMotorEnabled) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsMotorEnabled[mMapEntityToComponentIndex[jointEntity]] = isMotorEnabled; } // Return the Lower limit (minimum allowed rotation angle in radian) -inline decimal HingeJointComponents::getLowerLimit(Entity jointEntity) const { +inline decimal SliderJointComponents::getLowerLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLowerLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the Lower limit (minimum allowed rotation angle in radian) -inline void HingeJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { +inline void SliderJointComponents::setLowerLimit(Entity jointEntity, decimal lowerLimit) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mLowerLimit[mMapEntityToComponentIndex[jointEntity]] = lowerLimit; } // Return the upper limit (maximum translation distance) -inline decimal HingeJointComponents::getUpperLimit(Entity jointEntity) const { +inline decimal SliderJointComponents::getUpperLimit(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mUpperLimit[mMapEntityToComponentIndex[jointEntity]]; } // Set the upper limit (maximum translation distance) -inline void HingeJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { +inline void SliderJointComponents::setUpperLimit(Entity jointEntity, decimal upperLimit) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mUpperLimit[mMapEntityToComponentIndex[jointEntity]] = upperLimit; } // Return true if the lower limit is violated -inline bool HingeJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { +inline bool SliderJointComponents::getIsLowerLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the lower limit is violated -inline void HingeJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { +inline void SliderJointComponents::setIsLowerLimitViolated(Entity jointEntity, bool isLowerLimitViolated) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsLowerLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isLowerLimitViolated; } // Return true if the upper limit is violated -inline bool HingeJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { +inline bool SliderJointComponents::getIsUpperLimitViolated(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]]; } // Set to true if the upper limit is violated -inline void HingeJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { +inline void SliderJointComponents::setIsUpperLimitViolated(Entity jointEntity, bool isUpperLimitViolated) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mIsUpperLimitViolated[mMapEntityToComponentIndex[jointEntity]] = isUpperLimitViolated; } // Return the motor speed (in rad/s) -inline decimal HingeJointComponents::getMotorSpeed(Entity jointEntity) const { +inline decimal SliderJointComponents::getMotorSpeed(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mMotorSpeed[mMapEntityToComponentIndex[jointEntity]]; } // Set the motor speed (in rad/s) -inline void HingeJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { +inline void SliderJointComponents::setMotorSpeed(Entity jointEntity, decimal motorSpeed) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); mMotorSpeed[mMapEntityToComponentIndex[jointEntity]] = motorSpeed; } // Return the maximum motor torque (in Newtons) that can be applied to reach to desired motor speed -inline decimal HingeJointComponents::getMaxMotorTorque(Entity jointEntity) const { +inline decimal SliderJointComponents::getMaxMotorForce(Entity jointEntity) const { 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 -inline void HingeJointComponents::setMaxMotorTorque(Entity jointEntity, decimal maxMotorTorque) { +inline void SliderJointComponents::setMaxMotorForce(Entity jointEntity, decimal maxMotorForce) { 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; } -*/ } diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 2a05079c..dafc5dd8 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -36,16 +36,11 @@ const decimal SliderJoint::BETA = decimal(0.2); // Constructor SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo) - : Joint(entity, world, jointInfo), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), - mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), - mLowerLimit(jointInfo.minTranslationLimit), - mUpperLimit(jointInfo.maxTranslationLimit), mIsLowerLimitViolated(false), - mIsUpperLimitViolated(false), mMotorSpeed(jointInfo.motorSpeed), - mMaxMotorForce(jointInfo.maxMotorForce){ + : Joint(entity, world, jointInfo) { - assert(mUpperLimit >= decimal(0.0)); - assert(mLowerLimit <= decimal(0.0)); - assert(mMaxMotorForce >= decimal(0.0)); + assert(mWorld.mSliderJointsComponents.getUpperLimit(mEntity) >= decimal(0.0)); + assert(mWorld.mSliderJointsComponents.getLowerLimit(mEntity) <= decimal(0.0)); + assert(mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) >= decimal(0.0)); // Compute the local-space anchor point for each body 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 // TODO : Do not compute the inverse here, it has already been computed above - mSliderAxisBody1 = transform1.getOrientation().getInverse() * + Vector3 sliderAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.sliderAxisWorldSpace; - mSliderAxisBody1.normalize(); + sliderAxisBody1.normalize(); + mWorld.mSliderJointsComponents.setSliderAxisBody1(mEntity, sliderAxisBody1); } // Initialize before solving the constraint @@ -96,41 +92,56 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Vector from body center to the anchor point - mR1 = orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); - mR2 = orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); + mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(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) - 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 - mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = mSliderAxisWorld.cross(mN1); + Vector3 sliderAxisWorld = orientationBody1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity); + sliderAxisWorld.normalize(); + mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld); + 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 - decimal uDotSliderAxis = u.dot(mSliderAxisWorld); - decimal lowerLimitError = uDotSliderAxis - mLowerLimit; - decimal upperLimitError = mUpperLimit - uDotSliderAxis; - bool oldIsLowerLimitViolated = mIsLowerLimitViolated; - mIsLowerLimitViolated = lowerLimitError <= 0; - if (mIsLowerLimitViolated != oldIsLowerLimitViolated) { - mImpulseLowerLimit = 0.0; + decimal uDotSliderAxis = u.dot(sliderAxisWorld); + decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity); + decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis; + bool oldIsLowerLimitViolated = mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity); + bool isLowerLimitViolated = lowerLimitError <= 0; + mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated); + if (isLowerLimitViolated != oldIsLowerLimitViolated) { + mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); } - bool oldIsUpperLimitViolated = mIsUpperLimitViolated; - mIsUpperLimitViolated = upperLimitError <= 0; - if (mIsUpperLimitViolated != oldIsUpperLimitViolated) { - mImpulseUpperLimit = 0.0; + bool oldIsUpperLimitViolated = mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity); + bool isUpperLimitViolated = upperLimitError <= 0; + mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated); + if (isUpperLimitViolated != oldIsUpperLimitViolated) { + mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0)); } // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1)); + mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2)); + mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld)); + const Vector3 r1PlusU = r1 + u; + mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1)); + mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2)); + 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& i2 = mWorld.mSliderJointsComponents.getI2(mEntity); @@ -140,18 +151,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal sumInverseMass = body1MassInverse + body2MassInverse; - Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = i2 * mR2CrossN1; - Vector3 I2R2CrossN2 = i2 * mR2CrossN2; - const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); + Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2; + Vector3 I2R2CrossN1 = i2 * r2CrossN1; + Vector3 I2R2CrossN2 = i2 * r2CrossN2; + const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + r2CrossN1.dot(I2R2CrossN1); + const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + r2CrossN1.dot(I2R2CrossN2); + const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + r2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + r2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); inverseMassMatrixTranslation.setToZero(); @@ -166,8 +177,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa biasTranslation.setToZero(); decimal biasFactor = (BETA / constraintSolverData.timeStep); if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - biasTranslation.x = u.dot(mN1); - biasTranslation.y = u.dot(mN2); + biasTranslation.x = u.dot(n1); + biasTranslation.y = u.dot(n2); biasTranslation *= biasFactor; mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation); } @@ -190,35 +201,38 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa } // 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) - mInverseMassMatrixLimit = sumInverseMass + - mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); - mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); + decimal inverseMassMatrixLimit = sumInverseMass + + r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) + + r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis); + inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixLimit : decimal(0.0); + mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit); // 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) { - mBLowerLimit = biasFactor * lowerLimitError; + mWorld.mSliderJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError); } // 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) { - mBUpperLimit = biasFactor * upperLimitError; + mWorld.mSliderJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError); } } // 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) - mInverseMassMatrixMotor = sumInverseMass; - mInverseMassMatrixMotor = (mInverseMassMatrixMotor > decimal(0.0)) ? - decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); + decimal inverseMassMatrixMotor = sumInverseMass; + inverseMassMatrixMotor = (inverseMassMatrixMotor > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixMotor : decimal(0.0); + mWorld.mSliderJointsComponents.setInverseMassMatrixMotor(mEntity, inverseMassMatrixMotor); } // If warm-starting is not enabled @@ -229,9 +243,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); impulseTranslation.setToZero(); impulseRotation.setToZero(); - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; - mImpulseMotor = 0.0; + mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); + mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(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 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 - decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; - Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld; + decimal impulseLimits = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity); + Vector3 linearImpulseLimits = impulseLimits * mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity); // 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 Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - Vector3 linearImpulseBody1 = -mN1 * impulseTranslation.x - mN2 * impulseTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * impulseTranslation.x - - mR1PlusUCrossN2 * impulseTranslation.y; + Vector3 linearImpulseBody1 = -n1 * impulseTranslation.x - n2 * impulseTranslation.y; + Vector3 angularImpulseBody1 = -mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity) * impulseTranslation.x - + mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity) * impulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 angularImpulseBody1 += -impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 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 linearImpulseBody1 += impulseMotor; @@ -285,16 +302,16 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { w1 += mWorld.mSliderJointsComponents.getI1(mEntity) * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - Vector3 linearImpulseBody2 = mN1 * impulseTranslation.x + mN2 * impulseTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * impulseTranslation.x + - mR2CrossN2 * impulseTranslation.y; + Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y; + Vector3 angularImpulseBody2 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity) * impulseTranslation.x + + mWorld.mSliderJointsComponents.getR2CrossN2(mEntity) * impulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 += impulseRotation; // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2 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 linearImpulseBody2 += -impulseMotor; @@ -323,17 +340,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(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 decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); + const Vector3& sliderAxisWorld = mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity); + // --------------- Translation Constraints --------------- // // Compute J*v for the 2 translation constraints - const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + - mN1.dot(v2) + w2.dot(mR2CrossN1); - const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + - mN2.dot(v2) + w2.dot(mR2CrossN2); + const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) + + n1.dot(v2) + w2.dot(r2CrossN1); + const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) + + n2.dot(v2) + w2.dot(r2CrossN2); const Vector2 JvTranslation(el1, el2); // 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)); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x - - mR1PlusUCrossN2 * deltaLambda.y; + const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y; + Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x - + r1PlusUCrossN2 * deltaLambda.y; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; + const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y; + Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -381,32 +410,34 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Limits Constraints --------------- // - if (mIsLimitEnabled) { + if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity)) { + + const decimal inverseMassMatrixLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity); // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) { // Compute J*v for the lower limit constraint - const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - - mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); + const decimal JvLowerLimit = sliderAxisWorld.dot(v2) + r2CrossSliderAxis.dot(w2) - + sliderAxisWorld.dot(v1) - r1PlusUCrossSliderAxis.dot(w1); // Compute the Lagrange multiplier lambda for the lower limit constraint - decimal deltaLambdaLower = mInverseMassMatrixLimit * (-JvLowerLimit -mBLowerLimit); - decimal lambdaTemp = mImpulseLowerLimit; - mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); - deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + decimal deltaLambdaLower = inverseMassMatrixLimit * (-JvLowerLimit - mWorld.mSliderJointsComponents.getBLowerLimit(mEntity)); + decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity); + mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) + deltaLambdaLower, decimal(0.0))); + deltaLambdaLower = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) - lambdaTemp; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 - const Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody1 = -deltaLambdaLower * sliderAxisWorld; + const Vector3 angularImpulseBody1 = -deltaLambdaLower * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld; + const Vector3 angularImpulseBody2 = deltaLambdaLower * r2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -414,29 +445,29 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) { // Compute J*v for the upper limit constraint - const decimal JvUpperLimit = mSliderAxisWorld.dot(v1) + mR1PlusUCrossSliderAxis.dot(w1) - - mSliderAxisWorld.dot(v2) - mR2CrossSliderAxis.dot(w2); + const decimal JvUpperLimit = sliderAxisWorld.dot(v1) + r1PlusUCrossSliderAxis.dot(w1) + - sliderAxisWorld.dot(v2) - r2CrossSliderAxis.dot(w2); // Compute the Lagrange multiplier lambda for the upper limit constraint - decimal deltaLambdaUpper = mInverseMassMatrixLimit * (-JvUpperLimit -mBUpperLimit); - decimal lambdaTemp = mImpulseUpperLimit; - mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0)); - deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp; + decimal deltaLambdaUpper = inverseMassMatrixLimit * (-JvUpperLimit -mWorld.mSliderJointsComponents.getBUpperLimit(mEntity)); + decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity); + mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) + deltaLambdaUpper, decimal(0.0))); + deltaLambdaUpper = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - lambdaTemp; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 1 - const Vector3 linearImpulseBody1 = deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = deltaLambdaUpper * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody1 = deltaLambdaUpper * sliderAxisWorld; + const Vector3 angularImpulseBody1 = deltaLambdaUpper * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mR2CrossSliderAxis; + const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld; + const Vector3 angularImpulseBody2 = -deltaLambdaUpper * r2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -446,26 +477,26 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // --------------- Motor --------------- // - if (mIsMotorEnabled) { + if (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity)) { // 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 - const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep; - decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor - mMotorSpeed); - decimal lambdaTemp = mImpulseMotor; - mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); - deltaLambdaMotor = mImpulseMotor - lambdaTemp; + const decimal maxMotorImpulse = mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) * constraintSolverData.timeStep; + decimal deltaLambdaMotor = mWorld.mSliderJointsComponents.getInverseMassMatrixMotor(mEntity) * (-JvMotor - mWorld.mSliderJointsComponents.getMotorSpeed(mEntity)); + decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity); + mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, clamp(mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse)); + deltaLambdaMotor = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) - lambdaTemp; // 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 v1 += inverseMassBody1 * linearImpulseBody1; // 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 v2 += inverseMassBody2 * linearImpulseBody2; @@ -502,33 +533,47 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); // Vector from body center to the anchor point - mR1 = q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity); - mR2 = q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity); + mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(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) - 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 - mSliderAxisWorld = q1 * mSliderAxisBody1; - mSliderAxisWorld.normalize(); - mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = mSliderAxisWorld.cross(mN1); + Vector3 sliderAxisWorld = q1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity); + sliderAxisWorld.normalize(); + mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld); + mWorld.mSliderJointsComponents.setN1(mEntity, sliderAxisWorld.getOneUnitOrthogonalVector()); + mWorld.mSliderJointsComponents.setN2(mEntity, sliderAxisWorld.cross(n1)); // Check if the limit constraints are violated or not - decimal uDotSliderAxis = u.dot(mSliderAxisWorld); - decimal lowerLimitError = uDotSliderAxis - mLowerLimit; - decimal upperLimitError = mUpperLimit - uDotSliderAxis; - mIsLowerLimitViolated = lowerLimitError <= 0; - mIsUpperLimitViolated = upperLimitError <= 0; + decimal uDotSliderAxis = u.dot(sliderAxisWorld); + decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity); + decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis; + mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0); + mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0); // Compute the cross products used in the Jacobians - mR2CrossN1 = mR2.cross(mN1); - mR2CrossN2 = mR2.cross(mN2); - mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); - const Vector3 r1PlusU = mR1 + u; - mR1PlusUCrossN1 = (r1PlusU).cross(mN1); - mR1PlusUCrossN2 = (r1PlusU).cross(mN2); - mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); + mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1)); + mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2)); + mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld)); + const Vector3 r1PlusU = r1 + u; + mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1)); + mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2)); + 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 --------------- // @@ -540,18 +585,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); decimal sumInverseMass = body1MassInverse + body2MassInverse; - Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1; - Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2; - Vector3 I2R2CrossN1 = i2 * mR2CrossN1; - Vector3 I2R2CrossN2 = i2 * mR2CrossN2; - const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + - mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + - mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + - mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + - mR2CrossN2.dot(I2R2CrossN2); + Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1; + Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2; + Vector3 I2R2CrossN1 = i2 * r2CrossN1; + Vector3 I2R2CrossN2 = i2 * r2CrossN2; + const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + r2CrossN1.dot(I2R2CrossN1); + const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + r2CrossN1.dot(I2R2CrossN2); + const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + r2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + r2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity); inverseMassMatrixTranslation.setToZero(); @@ -562,15 +607,15 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } // 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 Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1 - const Vector3 linearImpulseBody1 = -mN1 * lambdaTranslation.x - mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * lambdaTranslation.x - - mR1PlusUCrossN2 * lambdaTranslation.y; + const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y; + Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x - + r1PlusUCrossN2 * lambdaTranslation.y; // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -582,9 +627,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 - const Vector3 linearImpulseBody2 = mN1 * lambdaTranslation.x + mN2 * lambdaTranslation.y; - Vector3 angularImpulseBody2 = mR2CrossN1 * lambdaTranslation.x + - mR2CrossN2 * lambdaTranslation.y; + const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y; + Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y; // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -659,29 +703,30 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // --------------- 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) const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); - mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + - mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) + - mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis); - mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? - decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0); + decimal inverseMassMatrixLimit = body1MassInverse + body2MassInverse + + r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) + + r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis); + inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixLimit : decimal(0.0); + mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit); } // If the lower limit is violated - if (mIsLowerLimitViolated) { + if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) { // 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 - const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody1 = -lambdaLowerLimit * sliderAxisWorld; + const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -693,8 +738,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 - const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = lambdaLowerLimit * mR2CrossSliderAxis; + const Vector3 linearImpulseBody2 = lambdaLowerLimit * sliderAxisWorld; + const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis; // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -707,14 +752,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } // If the upper limit is violated - if (mIsUpperLimitViolated) { + if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) { // 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 - const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody1 = lambdaUpperLimit * mR1PlusUCrossSliderAxis; + const Vector3 linearImpulseBody1 = lambdaUpperLimit * sliderAxisWorld; + const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; @@ -726,8 +771,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q1.normalize(); // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 - const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderAxisWorld; - const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mR2CrossSliderAxis; + const Vector3 linearImpulseBody2 = -lambdaUpperLimit * sliderAxisWorld; + const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis; // Apply the impulse to the body 2 const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; @@ -753,9 +798,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint */ 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 resetLimits(); @@ -769,8 +816,8 @@ void SliderJoint::enableLimit(bool isLimitEnabled) { */ void SliderJoint::enableMotor(bool isMotorEnabled) { - mIsMotorEnabled = isMotorEnabled; - mImpulseMotor = 0.0; + mWorld.mSliderJointsComponents.setIsMotorEnabled(mEntity, isMotorEnabled); + mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, decimal(0.0)); // Wake up the two bodies of the joint awakeBodies(); @@ -805,7 +852,7 @@ decimal SliderJoint::getTranslation() const { const Vector3 u = anchorBody2 - anchorBody1; // Compute the slider axis in world-space - Vector3 sliderAxisWorld = q1 * mSliderAxisBody1; + Vector3 sliderAxisWorld = q1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity); sliderAxisWorld.normalize(); // Compute and return the translation value @@ -818,11 +865,11 @@ decimal SliderJoint::getTranslation() const { */ 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 resetLimits(); @@ -835,11 +882,11 @@ void SliderJoint::setMinTranslationLimit(decimal lowerLimit) { */ 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 resetLimits(); @@ -850,8 +897,8 @@ void SliderJoint::setMaxTranslationLimit(decimal upperLimit) { void SliderJoint::resetLimits() { // Reset the accumulated impulses for the limits - mImpulseLowerLimit = 0.0; - mImpulseUpperLimit = 0.0; + mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0)); + mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0)); // Wake up the two bodies of the joint awakeBodies(); @@ -863,9 +910,9 @@ void SliderJoint::resetLimits() { */ 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 awakeBodies(); @@ -878,23 +925,82 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) { */ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { - if (maxMotorForce != mMaxMotorForce) { + const decimal maxForce = mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity); - assert(mMaxMotorForce >= decimal(0.0)); - mMaxMotorForce = maxMotorForce; + if (maxMotorForce != maxForce) { + + assert(maxForce >= decimal(0.0)); + mWorld.mSliderJointsComponents.setMaxMotorForce(mEntity, maxMotorForce); // Wake up the two bodies of the joint 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 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=" + - mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", sliderAxisBody1=" + mSliderAxisBody1.to_string() + + mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", sliderAxisBody1=" + mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity).to_string() + ", initOrientationDifferenceInv=" + - mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) + - ", maxMotorForce=" + std::to_string(mMaxMotorForce) + ", isLimitEnabled=" + - (mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}"; + mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + ", motorSpeed=" + std::to_string(getMotorSpeed()) + + ", maxMotorForce=" + std::to_string(getMaxMotorForce()) + ", isLimitEnabled=" + + (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity) ? "true" : "false") + ", isMotorEnabled=" + (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity) ? "true" : "false") + "}"; } diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index fc5b2e98..ac6a8e91 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -150,87 +150,6 @@ class SliderJoint : public Joint { // -------------------- 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 -------------------- // /// Reset the limits @@ -313,63 +232,6 @@ class SliderJoint : public Joint { 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 inline size_t SliderJoint::getSizeInBytes() const { return sizeof(SliderJoint); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 64a495ca..db5a289c 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -348,13 +348,15 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { + const SliderJointInfo& info = static_cast(jointInfo); + // 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); - void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, - sizeof(SliderJoint)); - const SliderJointInfo& info = static_cast(jointInfo); + void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint)); SliderJoint* joint = new (allocatedMemory) SliderJoint(entity, *this, info); newJoint = joint;