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

Add SliderJointComponents class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-09-13 07:15:48 +02:00
parent 06132e3d41
commit 0c0ff46d34
10 changed files with 1421 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -150,6 +150,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/components/BallAndSocketJointComponents.h"
"src/components/FixedJointComponents.h"
"src/components/HingeJointComponents.h"
"src/components/SliderJointComponents.h"
"src/collision/CollisionCallback.h"
"src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
@ -246,6 +247,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/components/BallAndSocketJointComponents.cpp"
"src/components/FixedJointComponents.cpp"
"src/components/HingeJointComponents.cpp"
"src/components/SliderJointComponents.cpp"
"src/collision/CollisionCallback.cpp"
"src/collision/OverlapCallback.cpp"
"src/mathematics/mathematics_functions.cpp"

6
src/components/HingeJointComponents.cpp
View File

@ -197,11 +197,11 @@ void HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, con
new (mI1 + index) Matrix3x3();
new (mI2 + index) Matrix3x3();
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 (mInverseMassMatrixRotation + index) Matrix3x3();
new (mInverseMassMatrixRotation + index) Matrix2x2();
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 (mHingeLocalAxisBody1 + index) Vector3(0, 0, 0);
new (mHingeLocalAxisBody2 + index) Vector3(0, 0, 0);

408
src/components/SliderJointComponents.cpp Normal file
View 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
View 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

184
src/constraint/SliderJoint.cpp
View File

@ -36,8 +36,7 @@ const decimal SliderJoint::BETA = decimal(0.2);
// Constructor
SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
: Joint(entity, world, jointInfo), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit),
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
const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
mWorld.mSliderJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.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:
//
@ -66,7 +65,7 @@ SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointI
// q10 = initial orientation of body 1
// r0 = initial rotation rotation from body 1 to body 2
// 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
// 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();
// Get the inertia tensor of bodies
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Vector from body center to the anchor point
mR1 = orientationBody1 * mLocalAnchorPointBody1;
mR2 = orientationBody2 * mLocalAnchorPointBody2;
mR1 = orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity);
mR2 = orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity);
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
@ -133,15 +132,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR1PlusUCrossN2 = (r1PlusU).cross(mN2);
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
// constraints (2x2 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = i2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -151,36 +153,40 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == 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
mBTranslation.setToZero();
Vector2& biasTranslation = mWorld.mSliderJointsComponents.getBiasTranslation(mEntity);
biasTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mBTranslation.x = u.dot(mN1);
mBTranslation.y = u.dot(mN2);
mBTranslation *= biasFactor;
biasTranslation.x = u.dot(mN1);
biasTranslation.y = u.dot(mN2);
biasTranslation *= biasFactor;
mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation);
}
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2;
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2);
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == 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
mBRotation.setToZero();
Vector3& biasRotation = mWorld.mSliderJointsComponents.getBiasRotation(mEntity);
biasRotation.setToZero();
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
const Quaternion qError = orientationBody2 * mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse();
mWorld.mSliderJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV());
}
// 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)
mInverseMassMatrixLimit = sumInverseMass +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
@ -219,8 +225,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
mImpulseTranslation.setToZero();
mImpulseRotation.setToZero();
Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity);
Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity);
impulseTranslation.setToZero();
impulseRotation.setToZero();
mImpulseLowerLimit = 0.0;
mImpulseUpperLimit = 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
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
Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x -
mR1PlusUCrossN2 * mImpulseTranslation.y;
Vector3 linearImpulseBody1 = -mN1 * impulseTranslation.x - mN2 * impulseTranslation.y;
Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * impulseTranslation.x -
mR1PlusUCrossN2 * impulseTranslation.y;
// 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
linearImpulseBody1 += linearImpulseLimits;
@ -271,15 +282,15 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Apply the impulse to the body 1
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
Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + mN2 * mImpulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x +
mR2CrossN2 * mImpulseTranslation.y;
Vector3 linearImpulseBody2 = mN1 * impulseTranslation.x + mN2 * impulseTranslation.y;
Vector3 angularImpulseBody2 = mR2CrossN1 * impulseTranslation.x +
mR2CrossN2 * impulseTranslation.y;
// 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
linearImpulseBody2 += -linearImpulseLimits;
@ -290,7 +301,7 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
w2 += mWorld.mSliderJointsComponents.getI2(mEntity) * angularImpulseBody2;
}
// Solve the velocity constraint
@ -309,6 +320,9 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
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
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
@ -323,8 +337,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const Vector2 JvTranslation(el1, el2);
// Compute the Lagrange multiplier lambda for the 2 translation constraints
Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation);
mImpulseTranslation += deltaLambda;
Vector2 deltaLambda = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity) * (-JvTranslation - mWorld.mSliderJointsComponents.getBiasTranslation(mEntity));
mWorld.mSliderJointsComponents.setImpulseTranslation(mEntity, deltaLambda + mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity));
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
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
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
const Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y;
@ -341,7 +355,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
w2 += i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
@ -349,20 +363,21 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
const Vector3 JvRotation = w2 - w1;
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation);
mImpulseRotation += deltaLambda2;
Vector3 deltaLambda2 = mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity) *
(-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
angularImpulseBody1 = -deltaLambda2;
// 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
angularImpulseBody2 = deltaLambda2;
// Apply the impulse to the body 2
w2 += mI2 * angularImpulseBody2;
w2 += i2 * angularImpulseBody2;
// --------------- Limits Constraints --------------- //
@ -387,7 +402,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
const Vector3 linearImpulseBody2 = deltaLambdaLower * mSliderAxisWorld;
@ -395,7 +410,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2;
w2 += mI2 * angularImpulseBody2;
w2 += i2 * angularImpulseBody2;
}
// If the upper limit is violated
@ -417,7 +432,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * mSliderAxisWorld;
@ -425,7 +440,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
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);
// Recompute the inertia tensor of bodies
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Vector from body center to the anchor point
mR1 = q1 * mLocalAnchorPointBody1;
mR2 = q2 * mLocalAnchorPointBody2;
mR1 = q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity);
mR2 = q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity);
// Compute the vector u (difference between anchor points)
const Vector3 u = x2 + mR2 - x1 - mR1;
@ -517,15 +532,18 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// --------------- 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
// constraints (2x2 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
Vector3 I2R2CrossN2 = mI2 * mR2CrossN2;
Vector3 I1R1PlusUCrossN1 = i1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = i1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = i2 * mR2CrossN1;
Vector3 I2R2CrossN2 = i2 * mR2CrossN2;
const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
mR2CrossN1.dot(I2R2CrossN1);
const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -535,18 +553,19 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == 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
const Vector2 translationError(u.dot(mN1), u.dot(mN2));
// 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
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
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mI1 * angularImpulseBody1;
Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
@ -569,7 +588,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
Vector3 w2 = mI2 * angularImpulseBody2;
Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
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
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2;
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2);
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse());
}
// Calculate difference in rotation
@ -602,7 +621,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// 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:
//
@ -616,13 +635,13 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// 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
angularImpulseBody1 = -lambdaRotation;
// Apply the impulse to the body 1
w1 = mI1 * angularImpulseBody1;
w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
@ -632,7 +651,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
angularImpulseBody2 = lambdaRotation;
// Apply the impulse to the body 2
w2 = mI2 * angularImpulseBody2;
w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
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 body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
mInverseMassMatrixLimit = body1MassInverse + body2MassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mR1PlusUCrossSliderAxis.dot(i1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(i2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 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
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
const Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
@ -679,7 +698,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
const Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
@ -699,7 +718,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
const Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
@ -712,7 +731,7 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Apply the impulse to the body 2
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
const Vector3 w2 = mI2 * angularImpulseBody2;
const Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
@ -779,8 +798,8 @@ decimal SliderJoint::getTranslation() const {
const Quaternion& q2 = transform2.getOrientation();
// Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1;
const Vector3 anchorBody2 = x2 + q2 * mLocalAnchorPointBody2;
const Vector3 anchorBody1 = x1 + q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity);
const Vector3 anchorBody2 = x2 + q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity);
// Compute the vector u (difference between anchor points)
const Vector3 u = anchorBody2 - anchorBody1;
@ -868,3 +887,14 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
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") + "}";
}

52
src/constraint/SliderJoint.h
View File

@ -150,36 +150,21 @@ class SliderJoint : public Joint {
// -------------------- Attributes -------------------- //
/// Anchor point of body 1 (in local-space coordinates of body 1)
Vector3 mLocalAnchorPointBody1;
/// Vector r1 in world-space coordinates
Vector3 mR1;
/// Anchor point of body 2 (in local-space coordinates of body 2)
Vector3 mLocalAnchorPointBody2;
/// Vector r2 in world-space coordinates
Vector3 mR2;
/// Slider axis (in local-space coordinates of body 1)
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
Vector3 mN1;
/// Second vector orthogonal to the slider axis and mN1 in local-space of body 1
Vector3 mN2;
/// Vector r1 in world-space coordinates
Vector3 mR1;
/// Vector r2 in world-space coordinates
Vector3 mR2;
/// Cross product of r2 and n1
Vector3 mR2CrossN1;
@ -198,36 +183,18 @@ class SliderJoint : public Joint {
/// Cross product of vector (r1 + u) and the slider axis
Vector3 mR1PlusUCrossSliderAxis;
/// Bias of the 2 translation constraints
Vector2 mBTranslation;
/// Bias of the 3 rotation constraints
Vector3 mBRotation;
/// Bias of the lower limit constraint
decimal mBLowerLimit;
/// Bias of the upper limit constraint
decimal mBUpperLimit;
/// Inverse of mass matrix K=JM^-1J^t for the 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)
decimal mInverseMassMatrixLimit;
/// Inverse of mass matrix K=JM^-1J^t for the motor
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
decimal mImpulseLowerLimit;
@ -408,17 +375,6 @@ inline size_t SliderJoint::getSizeInBytes() const {
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

4
src/engine/CollisionWorld.cpp
View File

@ -42,6 +42,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge
mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()),
mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()),
mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mHingeJointsComponents(mMemoryManager.getBaseAllocator()),
mSliderJointsComponents(mMemoryManager.getBaseAllocator()),
mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager),
mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr),
mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr),
@ -271,6 +272,9 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) {
if (mHingeJointsComponents.hasComponent(jointEntity)) {
mHingeJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
}
if (mSliderJointsComponents.hasComponent(jointEntity)) {
mSliderJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
}
}
// Return true if two bodies overlap

4
src/engine/CollisionWorld.h
View File

@ -41,6 +41,7 @@
#include "components/BallAndSocketJointComponents.h"
#include "components/FixedJointComponents.h"
#include "components/HingeJointComponents.h"
#include "components/SliderJointComponents.h"
#include "collision/CollisionCallback.h"
#include "collision/OverlapCallback.h"
@ -104,6 +105,9 @@ class CollisionWorld {
/// Hinge joints Components
HingeJointComponents mHingeJointsComponents;
/// Slider joints Components
SliderJointComponents mSliderJointsComponents;
/// Reference to the collision detection
CollisionDetectionSystem mCollisionDetection;

29
src/engine/DynamicsWorld.cpp
View File

@ -348,10 +348,18 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Slider joint
case JointType::SLIDERJOINT:
{
// Create a SliderJoint component
SliderJointComponents::SliderJointComponent sliderJointComponent;
mSliderJointsComponents.addComponent(entity, isJointDisabled, sliderJointComponent);
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(SliderJoint));
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;
}
@ -461,10 +469,25 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
size_t nbBytes = joint->getSizeInBytes();
Entity jointEntity = joint->getEntity();
// Destroy the corresponding entity and its components
// TODO : Make sure we remove all its components here
mJointsComponents.removeComponent(joint->getEntity());
mEntityManager.destroyEntity(joint->getEntity());
mJointsComponents.removeComponent(jointEntity);
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
joint->~Joint();

1
src/engine/DynamicsWorld.h
View File

@ -410,7 +410,6 @@ inline decimal DynamicsWorld::getTimeBeforeSleep() const {
return mTimeBeforeSleep;
}
// 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)