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

Working on SliderJointComponents class

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

1
src/components/HingeJointComponents.cpp
View File

@ -210,6 +210,7 @@ void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, con
new (mC2CrossA1 + index) Vector3(0, 0, 0);
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);

195
src/components/SliderJointComponents.cpp
View File

@ -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<Vector2*>(newInverseMassMatrixRotation + nbComponentsToAllocate);
Vector3* newBiasRotation = reinterpret_cast<Vector3*>(newBiasTranslation + nbComponentsToAllocate);
Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(newBiasRotation + nbComponentsToAllocate);
/*
Vector3* newHingeLocalAxisBody1 = reinterpret_cast<Vector3*>(newInitOrientationDifferenceInv + nbComponentsToAllocate);
Vector3* newHingeLocalAxisBody2 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody1 + nbComponentsToAllocate);
Vector3* newA1 = reinterpret_cast<Vector3*>(newHingeLocalAxisBody2 + nbComponentsToAllocate);
Vector3* newB2CrossA1 = reinterpret_cast<Vector3*>(newA1 + nbComponentsToAllocate);
Vector3* newC2CrossA1 = reinterpret_cast<Vector3*>(newB2CrossA1 + nbComponentsToAllocate);
decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(newC2CrossA1 + nbComponentsToAllocate);
Vector3* newSliderAxisBody1 = reinterpret_cast<Vector3*>(newInitOrientationDifferenceInv + nbComponentsToAllocate);
Vector3* newSliderAxisWorld = reinterpret_cast<Vector3*>(newSliderAxisBody1 + nbComponentsToAllocate);
Vector3* newR1 = reinterpret_cast<Vector3*>(newSliderAxisWorld + nbComponentsToAllocate);
Vector3* newR2 = reinterpret_cast<Vector3*>(newR1 + nbComponentsToAllocate);
Vector3* newN1 = reinterpret_cast<Vector3*>(newR2 + nbComponentsToAllocate);
Vector3* newN2 = reinterpret_cast<Vector3*>(newN1 + nbComponentsToAllocate);
decimal* newImpulseLowerLimit = reinterpret_cast<decimal*>(newN2 + nbComponentsToAllocate);
decimal* newImpulseUpperLimit = reinterpret_cast<decimal*>(newImpulseLowerLimit + nbComponentsToAllocate);
decimal* newImpulseMotor = reinterpret_cast<decimal*>(newImpulseUpperLimit + nbComponentsToAllocate);
decimal* newInverseMassMatrixLimitMotor = reinterpret_cast<decimal*>(newImpulseMotor + nbComponentsToAllocate);
decimal* newInverseMassMatrixMotor = reinterpret_cast<decimal*>(newInverseMassMatrixLimitMotor + nbComponentsToAllocate);
decimal* newInverseMassMatrixLimit = reinterpret_cast<decimal*>(newImpulseMotor + nbComponentsToAllocate);
decimal* newInverseMassMatrixMotor = reinterpret_cast<decimal*>(newInverseMassMatrixLimit + nbComponentsToAllocate);
decimal* newBLowerLimit = reinterpret_cast<decimal*>(newInverseMassMatrixMotor + nbComponentsToAllocate);
decimal* newBUpperLimit = reinterpret_cast<decimal*>(newBLowerLimit + nbComponentsToAllocate);
bool* newIsLimitEnabled = reinterpret_cast<bool*>(newBUpperLimit + nbComponentsToAllocate);
@ -95,8 +97,13 @@ void SliderJointComponents::allocate(uint32 nbComponentsToAllocate) {
bool* newIsLowerLimitViolated = reinterpret_cast<bool*>(newUpperLimit + nbComponentsToAllocate);
bool* newIsUpperLimitViolated = reinterpret_cast<bool*>(newIsLowerLimitViolated + nbComponentsToAllocate);
decimal* newMotorSpeed = reinterpret_cast<decimal*>(newIsUpperLimitViolated + nbComponentsToAllocate);
decimal* newMaxMotorTorque = reinterpret_cast<decimal*>(newMotorSpeed + nbComponentsToAllocate);
*/
decimal* newMaxMotorForce = reinterpret_cast<decimal*>(newMotorSpeed + nbComponentsToAllocate);
Vector3* newR2CrossN1 = reinterpret_cast<Vector3*>(newMaxMotorForce + nbComponentsToAllocate);
Vector3* newR2CrossN2 = reinterpret_cast<Vector3*>(newR2CrossN1 + nbComponentsToAllocate);
Vector3* newR2CrossSliderAxis = reinterpret_cast<Vector3*>(newR2CrossN2 + nbComponentsToAllocate);
Vector3* newR1PlusUCrossN1 = reinterpret_cast<Vector3*>(newR2CrossSliderAxis + nbComponentsToAllocate);
Vector3* newR1PlusUCrossN2 = reinterpret_cast<Vector3*>(newR1PlusUCrossN1 + nbComponentsToAllocate);
Vector3* newR1PlusUCrossSliderAxis = reinterpret_cast<Vector3*>(newR1PlusUCrossN2 + nbComponentsToAllocate);
// If there was already components before
if (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<Entity, uint32>(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<Entity, uint32>(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();
}

396
src/components/SliderJointComponents.h
View File

@ -92,21 +92,23 @@ class SliderJointComponents : public Components {
/// Inverse of the initial orientation difference between the two bodies
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;
}
*/
}

504
src/constraint/SliderJoint.cpp
View File

@ -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") + "}";
}

138
src/constraint/SliderJoint.h
View File

@ -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);

10
src/engine/DynamicsWorld.cpp
View File

@ -348,13 +348,15 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Slider joint
case JointType::SLIDERJOINT:
{
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(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<const SliderJointInfo&>(jointInfo);
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint));
SliderJoint* joint = new (allocatedMemory) SliderJoint(entity, *this, info);
newJoint = joint;