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