Add HingeJointComponents class
This commit is contained in:
Daniel Chappuis
parent
67d8411623
commit
06132e3d41
|
|
@ -149,6 +149,7 @@ SET (REACTPHYSICS3D_HEADERS
|
|||
"src/components/JointComponents.h"
|
||||
"src/components/BallAndSocketJointComponents.h"
|
||||
"src/components/FixedJointComponents.h"
|
||||
"src/components/HingeJointComponents.h"
|
||||
"src/collision/CollisionCallback.h"
|
||||
"src/collision/OverlapCallback.h"
|
||||
"src/mathematics/mathematics.h"
|
||||
|
|
@ -244,6 +245,7 @@ SET (REACTPHYSICS3D_SOURCES
|
|||
"src/components/JointComponents.cpp"
|
||||
"src/components/BallAndSocketJointComponents.cpp"
|
||||
"src/components/FixedJointComponents.cpp"
|
||||
"src/components/HingeJointComponents.cpp"
|
||||
"src/collision/CollisionCallback.cpp"
|
||||
"src/collision/OverlapCallback.cpp"
|
||||
"src/mathematics/mathematics_functions.cpp"
|
||||
|
|
|
|||
408
src/components/HingeJointComponents.cpp
Normal file
408
src/components/HingeJointComponents.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 "HingeJointComponents.h"
|
||||
#include "engine/EntityManager.h"
|
||||
#include "mathematics/Matrix3x3.h"
|
||||
#include <cassert>
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
HingeJointComponents::HingeJointComponents(MemoryAllocator& allocator)
|
||||
:Components(allocator, sizeof(Entity) + sizeof(HingeJoint*) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
|
||||
sizeof(Vector2) + sizeof(Matrix3x3) + sizeof(Matrix2x2) +
|
||||
sizeof(Vector3) + sizeof(Vector2) + 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 HingeJointComponents::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);
|
||||
HingeJoint** newJoints = reinterpret_cast<HingeJoint**>(newJointEntities + nbComponentsToAllocate);
|
||||
Vector3* newLocalAnchorPointBody1 = reinterpret_cast<Vector3*>(newJoints + nbComponentsToAllocate);
|
||||
Vector3* newLocalAnchorPointBody2 = reinterpret_cast<Vector3*>(newLocalAnchorPointBody1 + nbComponentsToAllocate);
|
||||
Vector3* newR1World = reinterpret_cast<Vector3*>(newLocalAnchorPointBody2 + nbComponentsToAllocate);
|
||||
Vector3* newR2World = reinterpret_cast<Vector3*>(newR1World + nbComponentsToAllocate);
|
||||
Matrix3x3* newI1 = reinterpret_cast<Matrix3x3*>(newR2World + nbComponentsToAllocate);
|
||||
Matrix3x3* newI2 = reinterpret_cast<Matrix3x3*>(newI1 + nbComponentsToAllocate);
|
||||
Vector3* newImpulseTranslation = reinterpret_cast<Vector3*>(newI2 + nbComponentsToAllocate);
|
||||
Vector2* newImpulseRotation = reinterpret_cast<Vector2*>(newImpulseTranslation + nbComponentsToAllocate);
|
||||
Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast<Matrix3x3*>(newImpulseRotation + nbComponentsToAllocate);
|
||||
Matrix2x2* newInverseMassMatrixRotation = reinterpret_cast<Matrix2x2*>(newInverseMassMatrixTranslation + nbComponentsToAllocate);
|
||||
Vector3* newBiasTranslation = reinterpret_cast<Vector3*>(newInverseMassMatrixRotation + nbComponentsToAllocate);
|
||||
Vector2* newBiasRotation = reinterpret_cast<Vector2*>(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(HingeJoint*));
|
||||
memcpy(newLocalAnchorPointBody1, mLocalAnchorPointBody1, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newLocalAnchorPointBody2, mLocalAnchorPointBody2, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newR1World, mR1World, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newR2World, mR2World, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newI1, mI1, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newI2, mI2, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newImpulseTranslation, mImpulseTranslation, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newImpulseRotation, mImpulseRotation, mNbComponents * sizeof(Vector2));
|
||||
memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix2x2));
|
||||
memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector2));
|
||||
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;
|
||||
mR1World = newR1World;
|
||||
mR2World = newR2World;
|
||||
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 HingeJointComponents::addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& 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 (mR1World + index) Vector3(0, 0, 0);
|
||||
new (mR2World + index) Vector3(0, 0, 0);
|
||||
new (mI1 + index) Matrix3x3();
|
||||
new (mI2 + index) Matrix3x3();
|
||||
new (mImpulseTranslation + index) Vector3(0, 0, 0);
|
||||
new (mImpulseRotation + index) Vector3(0, 0, 0);
|
||||
new (mInverseMassMatrixTranslation + index) Matrix3x3();
|
||||
new (mInverseMassMatrixRotation + index) Matrix3x3();
|
||||
new (mBiasTranslation + index) Vector3(0, 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 HingeJointComponents::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 (mR1World + destIndex) Vector3(mR1World[srcIndex]);
|
||||
new (mR2World + destIndex) Vector3(mR2World[srcIndex]);
|
||||
new (mI1 + destIndex) Matrix3x3(mI1[srcIndex]);
|
||||
new (mI2 + destIndex) Matrix3x3(mI2[srcIndex]);
|
||||
new (mImpulseTranslation + destIndex) Vector3(mImpulseTranslation[srcIndex]);
|
||||
new (mImpulseRotation + destIndex) Vector2(mImpulseRotation[srcIndex]);
|
||||
new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]);
|
||||
new (mInverseMassMatrixRotation + destIndex) Matrix2x2(mInverseMassMatrixRotation[srcIndex]);
|
||||
new (mBiasTranslation + destIndex) Vector3(mBiasTranslation[srcIndex]);
|
||||
new (mBiasRotation + destIndex) Vector2(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 HingeJointComponents::swapComponents(uint32 index1, uint32 index2) {
|
||||
|
||||
// Copy component 1 data
|
||||
Entity jointEntity1(mJointEntities[index1]);
|
||||
HingeJoint* joint1 = mJoints[index1];
|
||||
Vector3 localAnchorPointBody1(mLocalAnchorPointBody1[index1]);
|
||||
Vector3 localAnchorPointBody2(mLocalAnchorPointBody2[index1]);
|
||||
Vector3 r1World1(mR1World[index1]);
|
||||
Vector3 r2World1(mR2World[index1]);
|
||||
Matrix3x3 i11(mI1[index1]);
|
||||
Matrix3x3 i21(mI2[index1]);
|
||||
Vector3 impulseTranslation1(mImpulseTranslation[index1]);
|
||||
Vector2 impulseRotation1(mImpulseRotation[index1]);
|
||||
Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]);
|
||||
Matrix2x2 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]);
|
||||
Vector3 biasTranslation1(mBiasTranslation[index1]);
|
||||
Vector2 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 (mR1World + index2) Vector3(r1World1);
|
||||
new (mR2World + index2) Vector3(r2World1);
|
||||
new (mI1 + index2) Matrix3x3(i11);
|
||||
new (mI2 + index2) Matrix3x3(i21);
|
||||
new (mImpulseTranslation + index2) Vector3(impulseTranslation1);
|
||||
new (mImpulseRotation + index2) Vector2(impulseRotation1);
|
||||
new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1);
|
||||
new (mInverseMassMatrixRotation + index2) Matrix2x2(inverseMassMatrixRotation1);
|
||||
new (mBiasTranslation + index2) Vector3(biasTranslation1);
|
||||
new (mBiasRotation + index2) Vector2(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 HingeJointComponents::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();
|
||||
mR1World[index].~Vector3();
|
||||
mR2World[index].~Vector3();
|
||||
mI1[index].~Matrix3x3();
|
||||
mI2[index].~Matrix3x3();
|
||||
mImpulseTranslation[index].~Vector3();
|
||||
mImpulseRotation[index].~Vector2();
|
||||
mInverseMassMatrixTranslation[index].~Matrix3x3();
|
||||
mInverseMassMatrixRotation[index].~Matrix2x2();
|
||||
mBiasTranslation[index].~Vector3();
|
||||
mBiasRotation[index].~Vector2();
|
||||
mInitOrientationDifferenceInv[index].~Quaternion();
|
||||
mHingeLocalAxisBody1[index].~Vector3();
|
||||
mHingeLocalAxisBody2[index].~Vector3();
|
||||
mA1[index].~Vector3();
|
||||
mB2CrossA1[index].~Vector3();
|
||||
mC2CrossA1[index].~Vector3();
|
||||
}
|
||||
898
src/components/HingeJointComponents.h
Normal file
898
src/components/HingeJointComponents.h
Normal file
|
|
@ -0,0 +1,898 @@
|
|||
/********************************************************************************
|
||||
* 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_HINGE_JOINT_COMPONENTS_H
|
||||
#define REACTPHYSICS3D_HINGE_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 HingeJoint;
|
||||
enum class JointType;
|
||||
|
||||
// Class HingeJointComponents
|
||||
/**
|
||||
* This class represent the component of the ECS with data for the HingeJoint.
|
||||
*/
|
||||
class HingeJointComponents : public Components {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Array of joint entities
|
||||
Entity* mJointEntities;
|
||||
|
||||
/// Array of pointers to the joints
|
||||
HingeJoint** 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;
|
||||
|
||||
/// Vector from center of body 2 to anchor point in world-space
|
||||
Vector3* mR1World;
|
||||
|
||||
/// Vector from center of body 2 to anchor point in world-space
|
||||
Vector3* mR2World;
|
||||
|
||||
/// 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
|
||||
Vector3* mImpulseTranslation;
|
||||
|
||||
/// Accumulate impulse for the 3 rotation constraints
|
||||
Vector2* mImpulseRotation;
|
||||
|
||||
/// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
|
||||
Matrix3x3* mInverseMassMatrixTranslation;
|
||||
|
||||
/// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
|
||||
Matrix2x2* mInverseMassMatrixRotation;
|
||||
|
||||
/// Bias vector for the 3 translation constraints
|
||||
Vector3* mBiasTranslation;
|
||||
|
||||
/// Bias vector for the 3 rotation constraints
|
||||
Vector2* 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 HingeJointComponent {
|
||||
|
||||
bool isLimitEnabled;
|
||||
bool isMotorEnabled;
|
||||
decimal lowerLimit;
|
||||
decimal upperLimit;
|
||||
decimal motorSpeed;
|
||||
decimal maxMotorTorque;
|
||||
|
||||
/// Constructor
|
||||
HingeJointComponent(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
|
||||
HingeJointComponents(MemoryAllocator& allocator);
|
||||
|
||||
/// Destructor
|
||||
virtual ~HingeJointComponents() override = default;
|
||||
|
||||
/// Add a component
|
||||
void addComponent(Entity jointEntity, bool isSleeping, const HingeJointComponent& component);
|
||||
|
||||
/// Return a pointer to a given joint
|
||||
HingeJoint* getJoint(Entity jointEntity) const;
|
||||
|
||||
/// Set the joint pointer to a given joint
|
||||
void setJoint(Entity jointEntity, HingeJoint* 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 vector from center of body 1 to anchor point in world-space
|
||||
const Vector3& getR1World(Entity jointEntity) const;
|
||||
|
||||
/// Set the vector from center of body 1 to anchor point in world-space
|
||||
void setR1World(Entity jointEntity, const Vector3& r1World);
|
||||
|
||||
/// Return the vector from center of body 2 to anchor point in world-space
|
||||
const Vector3& getR2World(Entity jointEntity) const;
|
||||
|
||||
/// Set the vector from center of body 2 to anchor point in world-space
|
||||
void setR2World(Entity jointEntity, const Vector3& r2World);
|
||||
|
||||
/// 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
|
||||
Vector3& getImpulseTranslation(Entity jointEntity);
|
||||
|
||||
/// Set the translation impulse
|
||||
void setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation);
|
||||
|
||||
/// Return the translation impulse
|
||||
Vector2& getImpulseRotation(Entity jointEntity);
|
||||
|
||||
/// Set the translation impulse
|
||||
void setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation);
|
||||
|
||||
/// Return the translation inverse mass matrix of the constraint
|
||||
Matrix3x3& getInverseMassMatrixTranslation(Entity jointEntity);
|
||||
|
||||
/// Set the translation inverse mass matrix of the constraint
|
||||
void setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix);
|
||||
|
||||
/// Return the rotation inverse mass matrix of the constraint
|
||||
Matrix2x2& getInverseMassMatrixRotation(Entity jointEntity);
|
||||
|
||||
/// Set the rotation inverse mass matrix of the constraint
|
||||
void setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix);
|
||||
|
||||
/// Return the translation bias
|
||||
Vector3& getBiasTranslation(Entity jointEntity);
|
||||
|
||||
/// Set the translation impulse
|
||||
void setBiasTranslation(Entity jointEntity, const Vector3& impulseTranslation);
|
||||
|
||||
/// Return the rotation bias
|
||||
Vector2& getBiasRotation(Entity jointEntity);
|
||||
|
||||
/// Set the rotation impulse
|
||||
void setBiasRotation(Entity jointEntity, const Vector2& 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 HingeJoint* HingeJointComponents::getJoint(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mJoints[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the joint pointer to a given joint
|
||||
inline void HingeJointComponents::setJoint(Entity jointEntity, HingeJoint* 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& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2;
|
||||
}
|
||||
|
||||
// Return the vector from center of body 1 to anchor point in world-space
|
||||
inline const Vector3& HingeJointComponents::getR1World(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mR1World[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the vector from center of body 1 to anchor point in world-space
|
||||
inline void HingeJointComponents::setR1World(Entity jointEntity, const Vector3& r1World) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mR1World[mMapEntityToComponentIndex[jointEntity]] = r1World;
|
||||
}
|
||||
|
||||
// Return the vector from center of body 2 to anchor point in world-space
|
||||
inline const Vector3& HingeJointComponents::getR2World(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mR2World[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the vector from center of body 2 to anchor point in world-space
|
||||
inline void HingeJointComponents::setR2World(Entity jointEntity, const Vector3& r2World) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mR2World[mMapEntityToComponentIndex[jointEntity]] = r2World;
|
||||
}
|
||||
|
||||
// Return the inertia tensor of body 1 (in world-space coordinates)
|
||||
inline const Matrix3x3& HingeJointComponents::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 HingeJointComponents::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& HingeJointComponents::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 HingeJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mI2[mMapEntityToComponentIndex[jointEntity]] = i2;
|
||||
}
|
||||
|
||||
// Return the translation impulse
|
||||
inline Vector3& HingeJointComponents::getImpulseTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void HingeJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the translation impulse
|
||||
inline Vector2& HingeJointComponents::getImpulseRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void HingeJointComponents::setImpulseRotation(Entity jointEntity, const Vector2& impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the translation inverse mass matrix of the constraint
|
||||
inline Matrix3x3& HingeJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
|
||||
// Set the translation inverse mass matrix of the constraint
|
||||
inline void HingeJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
|
||||
}
|
||||
|
||||
// Return the rotation inverse mass matrix of the constraint
|
||||
inline Matrix2x2& HingeJointComponents::getInverseMassMatrixRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation inverse mass matrix of the constraint
|
||||
inline void HingeJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix2x2& inverseMassMatrix) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
|
||||
}
|
||||
|
||||
// Return the translation bias
|
||||
inline Vector3& HingeJointComponents::getBiasTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void HingeJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the rotation bias
|
||||
inline Vector2 &HingeJointComponents::getBiasRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mBiasRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation impulse
|
||||
inline void HingeJointComponents::setBiasRotation(Entity jointEntity, const Vector2& impulseRotation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation;
|
||||
}
|
||||
|
||||
// Return the initial orientation difference
|
||||
inline Quaternion& HingeJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation impulse
|
||||
inline void HingeJointComponents::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
|
||||
|
|
@ -35,34 +35,33 @@ using namespace reactphysics3d;
|
|||
const decimal HingeJoint::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo)
|
||||
: Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0),
|
||||
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
|
||||
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
|
||||
mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit),
|
||||
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
|
||||
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorTorque(jointInfo.maxMotorTorque) {
|
||||
HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world, jointInfo) {
|
||||
|
||||
assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0) * PI);
|
||||
assert(mUpperLimit >= decimal(0) && mUpperLimit <= decimal(2.0) * PI);
|
||||
const decimal lowerLimit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
const decimal upperLimit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity);
|
||||
assert(lowerLimit <= decimal(0) && lowerLimit >= decimal(-2.0) * PI);
|
||||
assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0) * PI);
|
||||
|
||||
// Compute the local-space anchor point for each body
|
||||
Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||
mWorld.mHingeJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
mWorld.mHingeJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
|
||||
// Compute the local-space hinge axis
|
||||
mHingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
||||
mHingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
||||
mHingeLocalAxisBody1.normalize();
|
||||
mHingeLocalAxisBody2.normalize();
|
||||
Vector3 hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
||||
Vector3 hingeLocalAxisBody2 = transform2.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
|
||||
hingeLocalAxisBody1.normalize();
|
||||
hingeLocalAxisBody2.normalize();
|
||||
mWorld.mHingeJointsComponents.setHingeLocalAxisBody1(mEntity, hingeLocalAxisBody1);
|
||||
mWorld.mHingeJointsComponents.setHingeLocalAxisBody2(mEntity, hingeLocalAxisBody2);
|
||||
|
||||
// Compute the inverse of the initial orientation difference between the two bodies
|
||||
mInitOrientationDifferenceInv = transform2.getOrientation() *
|
||||
Quaternion initOrientationDifferenceInv = transform2.getOrientation() *
|
||||
transform1.getOrientation().getInverse();
|
||||
mInitOrientationDifferenceInv.normalize();
|
||||
mInitOrientationDifferenceInv.inverse();
|
||||
initOrientationDifferenceInv.normalize();
|
||||
initOrientationDifferenceInv.inverse();
|
||||
mWorld.mHingeJointsComponents.setInitOrientationDifferenceInv(mEntity, initOrientationDifferenceInv);
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
|
|
@ -83,43 +82,46 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mI1 = body1->getInertiaTensorInverseWorld();
|
||||
mI2 = body2->getInertiaTensorInverseWorld();
|
||||
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||
mR2World = orientationBody2 * mLocalAnchorPointBody2;
|
||||
mWorld.mHingeJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mHingeJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mLowerLimit;
|
||||
decimal upperLimitError = mUpperLimit - hingeAngle;
|
||||
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
|
||||
mIsLowerLimitViolated = lowerLimitError <= 0;
|
||||
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||
mImpulseLowerLimit = 0.0;
|
||||
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
|
||||
bool oldIsLowerLimitViolated = mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity);
|
||||
bool isLowerLimitViolated = lowerLimitError <= 0;
|
||||
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated);
|
||||
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
|
||||
mIsUpperLimitViolated = upperLimitError <= 0;
|
||||
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||
mImpulseUpperLimit = 0.0;
|
||||
bool oldIsUpperLimitViolated = mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity);
|
||||
bool isUpperLimitViolated = upperLimitError <= 0;
|
||||
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated);
|
||||
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
mA1 = orientationBody1 * mHingeLocalAxisBody1;
|
||||
Vector3 a2 = orientationBody2 * mHingeLocalAxisBody2;
|
||||
mA1.normalize();
|
||||
Vector3 a1 = orientationBody1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
|
||||
Vector3 a2 = orientationBody2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
|
||||
a1.normalize();
|
||||
a2.normalize();
|
||||
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
mB2CrossA1 = b2.cross(mA1);
|
||||
mC2CrossA1 = c2.cross(mA1);
|
||||
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2.cross(a1));
|
||||
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2.cross(a1));
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR2World(mEntity));
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
|
||||
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
|
||||
|
|
@ -128,78 +130,93 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
|
||||
mInverseMassMatrixTranslation.setToZero();
|
||||
skewSymmetricMatrixU1 * mWorld.mHingeJointsComponents.getI1(mEntity) * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mWorld.mHingeJointsComponents.getI2(mEntity) * skewSymmetricMatrixU2.getTranspose();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the translation constraints
|
||||
mBTranslation.setToZero();
|
||||
Vector3& bTranslation = mWorld.mHingeJointsComponents.getBiasTranslation(mEntity);
|
||||
bTranslation.setToZero();
|
||||
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
|
||||
bTranslation = biasFactor * (x2 + mWorld.mHingeJointsComponents.getR2World(mEntity) - x1 - mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
mWorld.mHingeJointsComponents.setBiasTranslation(mEntity, bTranslation);
|
||||
}
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1;
|
||||
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
|
||||
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
|
||||
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
|
||||
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
|
||||
mB2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
|
||||
mB2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
|
||||
mC2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
|
||||
mC2CrossA1.dot(I2C2CrossA1);
|
||||
Vector3 i1B2CrossA1 = i1 * b2CrossA1;
|
||||
Vector3 i1C2CrossA1 = i1 * c2CrossA1;
|
||||
Vector3 i2B2CrossA1 = i2 * b2CrossA1;
|
||||
Vector3 i2C2CrossA1 = i2 * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(i1B2CrossA1) +
|
||||
b2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(i1C2CrossA1) +
|
||||
b2CrossA1.dot(i2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(i1B2CrossA1) +
|
||||
c2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) +
|
||||
c2CrossA1.dot(i2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mInverseMassMatrixRotation.setToZero();
|
||||
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixRotation = matrixKRotation.getInverse();
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the rotation constraints
|
||||
mBRotation.setToZero();
|
||||
Vector2& biasRotation = mWorld.mHingeJointsComponents.getBiasRotation(mEntity);
|
||||
biasRotation.setToZero();
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBRotation = biasFactor * Vector2(mA1.dot(b2), mA1.dot(c2));
|
||||
mWorld.mHingeJointsComponents.setBiasRotation(mEntity, biasFactor * Vector2(a1.dot(b2), a1.dot(c2)));
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!constraintSolverData.isWarmStartingActive) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
mImpulseTranslation.setToZero();
|
||||
mImpulseRotation.setToZero();
|
||||
mImpulseLowerLimit = 0.0;
|
||||
mImpulseUpperLimit = 0.0;
|
||||
mImpulseMotor = 0.0;
|
||||
Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
|
||||
Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
|
||||
impulseTranslation.setToZero();
|
||||
impulseRotation.setToZero();
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
|
||||
}
|
||||
|
||||
// If the motor or limits are enabled
|
||||
if (mIsMotorEnabled || (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated))) {
|
||||
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) ||
|
||||
(mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) ||
|
||||
mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)))) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
||||
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
|
||||
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
|
||||
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
|
||||
decimal inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
|
||||
|
||||
if (mIsLimitEnabled) {
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mBLowerLimit = 0.0;
|
||||
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBLowerLimit = biasFactor * lowerLimitError;
|
||||
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError);
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mBUpperLimit = 0.0;
|
||||
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBUpperLimit = biasFactor * upperLimitError;
|
||||
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -225,18 +242,27 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
|||
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
|
||||
const Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
|
||||
|
||||
const decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
const decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
|
||||
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
|
||||
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
|
||||
Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mWorld.mHingeJointsComponents.getC2CrossA1(mEntity) * impulseRotation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
|
||||
const Vector3 limitsImpulse = (mImpulseUpperLimit - mImpulseLowerLimit) * mA1;
|
||||
const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint
|
||||
const Vector3 motorImpulse = -mImpulseMotor * mA1;
|
||||
const Vector3 motorImpulse = -mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
|
||||
Vector3 linearImpulseBody1 = -mImpulseTranslation;
|
||||
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
|
||||
Vector3 linearImpulseBody1 = -impulseTranslation;
|
||||
Vector3 angularImpulseBody1 = impulseTranslation.cross(mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 += rotationImpulse;
|
||||
|
|
@ -249,10 +275,10 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
|||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += mWorld.mHingeJointsComponents.getI1(mEntity) * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
|
||||
Vector3 angularImpulseBody2 = -impulseTranslation.cross(mWorld.mHingeJointsComponents.getR2World(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 += -rotationImpulse;
|
||||
|
|
@ -264,8 +290,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
|||
angularImpulseBody2 += -motorImpulse;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mImpulseTranslation;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
v2 += inverseMassBody2 * impulseTranslation;
|
||||
w2 += mWorld.mHingeJointsComponents.getI2(mEntity) * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
|
|
@ -288,136 +314,158 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
|
||||
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
const decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambdaTranslation = mInverseMassMatrixTranslation *
|
||||
(-JvTranslation - mBTranslation);
|
||||
mImpulseTranslation += deltaLambdaTranslation;
|
||||
const Vector3 deltaLambdaTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity) *
|
||||
(-JvTranslation - mWorld.mHingeJointsComponents.getBiasTranslation(mEntity));
|
||||
mWorld.mHingeJointsComponents.setImpulseTranslation(mEntity, deltaLambdaTranslation + mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(mR1World);
|
||||
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(mR2World);
|
||||
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World);
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * deltaLambdaTranslation;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 rotation constraints
|
||||
const Vector2 JvRotation(-mB2CrossA1.dot(w1) + mB2CrossA1.dot(w2),
|
||||
-mC2CrossA1.dot(w1) + mC2CrossA1.dot(w2));
|
||||
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
|
||||
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
|
||||
Vector2 deltaLambdaRotation = mInverseMassMatrixRotation * (-JvRotation - mBRotation);
|
||||
mImpulseRotation += deltaLambdaRotation;
|
||||
Vector2 deltaLambdaRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity) *
|
||||
(-JvRotation - mWorld.mHingeJointsComponents.getBiasRotation(mEntity));
|
||||
mWorld.mHingeJointsComponents.setImpulseRotation(mEntity, deltaLambdaRotation + mWorld.mHingeJointsComponents.getImpulseRotation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 = -mB2CrossA1 * deltaLambdaRotation.x -
|
||||
mC2CrossA1 * deltaLambdaRotation.y;
|
||||
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x -
|
||||
c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 = mB2CrossA1 * deltaLambdaRotation.x +
|
||||
mC2CrossA1 * deltaLambdaRotation.y;
|
||||
angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mIsLimitEnabled) {
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mIsLowerLimitViolated) {
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
|
||||
// Compute J*v for the lower limit constraint
|
||||
const decimal JvLowerLimit = (w2 - w1).dot(mA1);
|
||||
const decimal JvLowerLimit = (w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = mInverseMassMatrixLimitMotor * (-JvLowerLimit -mBLowerLimit);
|
||||
decimal lambdaTemp = mImpulseLowerLimit;
|
||||
mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0));
|
||||
deltaLambdaLower = mImpulseLowerLimit - lambdaTemp;
|
||||
decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mWorld.mHingeJointsComponents.getBLowerLimit(mEntity));
|
||||
decimal lambdaTemp = impulseLowerLimit;
|
||||
impulseLowerLimit = std::max(impulseLowerLimit + deltaLambdaLower, decimal(0.0));
|
||||
deltaLambdaLower = impulseLowerLimit - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, impulseLowerLimit);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * mA1;
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * mA1;
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mIsUpperLimitViolated) {
|
||||
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
|
||||
|
||||
// Compute J*v for the upper limit constraint
|
||||
const decimal JvUpperLimit = -(w2 - w1).dot(mA1);
|
||||
const decimal JvUpperLimit = -(w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal deltaLambdaUpper = mInverseMassMatrixLimitMotor * (-JvUpperLimit -mBUpperLimit);
|
||||
decimal lambdaTemp = mImpulseUpperLimit;
|
||||
mImpulseUpperLimit = std::max(mImpulseUpperLimit + deltaLambdaUpper, decimal(0.0));
|
||||
deltaLambdaUpper = mImpulseUpperLimit - lambdaTemp;
|
||||
decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mWorld.mHingeJointsComponents.getBUpperLimit(mEntity));
|
||||
decimal lambdaTemp = impulseUpperLimit;
|
||||
impulseUpperLimit = std::max(impulseUpperLimit + deltaLambdaUpper, decimal(0.0));
|
||||
deltaLambdaUpper = impulseUpperLimit - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, impulseUpperLimit);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * mA1;
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * mA1;
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Motor --------------- //
|
||||
|
||||
// If the motor is enabled
|
||||
if (mIsMotorEnabled) {
|
||||
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity)) {
|
||||
|
||||
decimal impulseMotor = mWorld.mHingeJointsComponents.getImpulseMotor(mEntity);
|
||||
|
||||
// Compute J*v for the motor
|
||||
const decimal JvMotor = mA1.dot(w1 - w2);
|
||||
const decimal JvMotor = a1.dot(w1 - w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the motor
|
||||
const decimal maxMotorImpulse = mMaxMotorTorque * constraintSolverData.timeStep;
|
||||
decimal deltaLambdaMotor = mInverseMassMatrixLimitMotor * (-JvMotor - mMotorSpeed);
|
||||
decimal lambdaTemp = mImpulseMotor;
|
||||
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
|
||||
const decimal maxMotorImpulse = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity) * constraintSolverData.timeStep;
|
||||
decimal deltaLambdaMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity) * (-JvMotor - mWorld.mHingeJointsComponents.getMotorSpeed(mEntity));
|
||||
decimal lambdaTemp = impulseMotor;
|
||||
impulseMotor = clamp(impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
deltaLambdaMotor = impulseMotor - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, impulseMotor);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * mA1;
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaMotor * mA1;
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -436,6 +484,17 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
|
||||
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
|
||||
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
|
||||
|
||||
Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
|
||||
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
|
||||
|
|
@ -447,35 +506,38 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mI1 = body1->getInertiaTensorInverseWorld();
|
||||
mI2 = body2->getInertiaTensorInverseWorld();
|
||||
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = q1 * mLocalAnchorPointBody1;
|
||||
mR2World = q2 * mLocalAnchorPointBody2;
|
||||
mWorld.mHingeJointsComponents.setR1World(mEntity, q1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mHingeJointsComponents.setR2World(mEntity, q2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mLowerLimit;
|
||||
decimal upperLimitError = mUpperLimit - hingeAngle;
|
||||
mIsLowerLimitViolated = lowerLimitError <= 0;
|
||||
mIsUpperLimitViolated = upperLimitError <= 0;
|
||||
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
|
||||
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0);
|
||||
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0);
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
mA1 = q1 * mHingeLocalAxisBody1;
|
||||
Vector3 a2 = q2 * mHingeLocalAxisBody2;
|
||||
mA1.normalize();
|
||||
a1 = q1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
|
||||
Vector3 a2 = q2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
|
||||
a1.normalize();
|
||||
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
|
||||
a2.normalize();
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
mB2CrossA1 = b2.cross(mA1);
|
||||
mC2CrossA1 = c2.cross(mA1);
|
||||
b2CrossA1 = b2.cross(a1);
|
||||
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2CrossA1);
|
||||
c2CrossA1 = c2.cross(a1);
|
||||
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2CrossA1);
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
|
|
@ -486,27 +548,29 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
|
||||
mInverseMassMatrixTranslation.setToZero();
|
||||
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, inverseMassMatrixTranslation);
|
||||
}
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + mR2World - x1 - mR1World;
|
||||
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 lambdaTranslation = mInverseMassMatrixTranslation * (-errorTranslation);
|
||||
const Vector3 lambdaTranslation = inverseMassMatrixTranslation * (-errorTranslation);
|
||||
|
||||
// Compute the impulse of body 1
|
||||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mR1World);
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
|
||||
|
||||
// Compute the pseudo velocity of 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;
|
||||
|
|
@ -514,11 +578,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mR2World);
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
|
||||
Vector3 w2 = mI2 * angularImpulseBody2;
|
||||
Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
|
|
@ -528,46 +592,47 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = mI1 * mB2CrossA1;
|
||||
Vector3 I1C2CrossA1 = mI1 * mC2CrossA1;
|
||||
Vector3 I2B2CrossA1 = mI2 * mB2CrossA1;
|
||||
Vector3 I2C2CrossA1 = mI2 * mC2CrossA1;
|
||||
const decimal el11 = mB2CrossA1.dot(I1B2CrossA1) +
|
||||
mB2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = mB2CrossA1.dot(I1C2CrossA1) +
|
||||
mB2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = mC2CrossA1.dot(I1B2CrossA1) +
|
||||
mC2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = mC2CrossA1.dot(I1C2CrossA1) +
|
||||
mC2CrossA1.dot(I2C2CrossA1);
|
||||
Vector3 I1B2CrossA1 = i1 * b2CrossA1;
|
||||
Vector3 I1C2CrossA1 = i1 * c2CrossA1;
|
||||
Vector3 I2B2CrossA1 = i2 * b2CrossA1;
|
||||
Vector3 I2C2CrossA1 = i2 * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
|
||||
b2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
|
||||
b2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
|
||||
c2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
|
||||
c2CrossA1.dot(I2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mInverseMassMatrixRotation.setToZero();
|
||||
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixRotation = matrixKRotation.getInverse();
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the position error for the 3 rotation constraints
|
||||
const Vector2 errorRotation = Vector2(mA1.dot(b2), mA1.dot(c2));
|
||||
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector2 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
|
||||
Vector2 lambdaRotation = inverseMassMatrixRotation * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -mB2CrossA1 * lambdaRotation.x - mC2CrossA1 * lambdaRotation.y;
|
||||
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
w1 = mI1 * angularImpulseBody1;
|
||||
w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
angularImpulseBody2 = mB2CrossA1 * lambdaRotation.x + mC2CrossA1 * lambdaRotation.y;
|
||||
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
w2 = mI2 * angularImpulseBody2;
|
||||
w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
|
|
@ -575,37 +640,40 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mIsLimitEnabled) {
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
|
||||
decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
|
||||
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
mInverseMassMatrixLimitMotor = mA1.dot(mI1 * mA1) + mA1.dot(mI2 * mA1);
|
||||
mInverseMassMatrixLimitMotor = (mInverseMassMatrixLimitMotor > 0.0) ?
|
||||
decimal(1.0) / mInverseMassMatrixLimitMotor : decimal(0.0);
|
||||
inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
|
||||
}
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mIsLowerLimitViolated) {
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal lambdaLowerLimit = mInverseMassMatrixLimitMotor * (-lowerLimitError );
|
||||
decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * mA1;
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mI1 * angularImpulseBody1;
|
||||
const Vector3 w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * mA1;
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mI2 * angularImpulseBody2;
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
|
|
@ -613,26 +681,26 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mIsUpperLimitViolated) {
|
||||
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal lambdaUpperLimit = mInverseMassMatrixLimitMotor * (-upperLimitError);
|
||||
decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * mA1;
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mI1 * angularImpulseBody1;
|
||||
const Vector3 w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * mA1;
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mI2 * angularImpulseBody2;
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
|
|
@ -654,9 +722,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
*/
|
||||
void HingeJoint::enableLimit(bool isLimitEnabled) {
|
||||
|
||||
if (isLimitEnabled != mIsLimitEnabled) {
|
||||
if (isLimitEnabled != mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
mIsLimitEnabled = isLimitEnabled;
|
||||
mWorld.mHingeJointsComponents.setIsLimitEnabled(mEntity, isLimitEnabled);
|
||||
|
||||
// Reset the limits
|
||||
resetLimits();
|
||||
|
|
@ -670,8 +738,8 @@ void HingeJoint::enableLimit(bool isLimitEnabled) {
|
|||
*/
|
||||
void HingeJoint::enableMotor(bool isMotorEnabled) {
|
||||
|
||||
mIsMotorEnabled = isMotorEnabled;
|
||||
mImpulseMotor = 0.0;
|
||||
mWorld.mHingeJointsComponents.setIsMotorEnabled(mEntity, isMotorEnabled);
|
||||
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
|
||||
|
||||
// Wake up the two bodies of the joint
|
||||
awakeBodies();
|
||||
|
|
@ -683,11 +751,13 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
|
|||
*/
|
||||
void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
|
||||
|
||||
assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0 * PI));
|
||||
const decimal limit = mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
|
||||
if (lowerLimit != mLowerLimit) {
|
||||
assert(limit <= decimal(0.0) && limit >= decimal(-2.0) * PI);
|
||||
|
||||
mLowerLimit = lowerLimit;
|
||||
if (lowerLimit != limit) {
|
||||
|
||||
mWorld.mHingeJointsComponents.setLowerLimit(mEntity, lowerLimit);
|
||||
|
||||
// Reset the limits
|
||||
resetLimits();
|
||||
|
|
@ -700,11 +770,13 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
|
|||
*/
|
||||
void HingeJoint::setMaxAngleLimit(decimal upperLimit) {
|
||||
|
||||
assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0 * PI));
|
||||
const decimal limit = mWorld.mHingeJointsComponents.getUpperLimit(mEntity);
|
||||
|
||||
if (upperLimit != mUpperLimit) {
|
||||
assert(limit >= decimal(0) && limit <= decimal(2.0) * PI);
|
||||
|
||||
mUpperLimit = upperLimit;
|
||||
if (upperLimit != limit) {
|
||||
|
||||
mWorld.mHingeJointsComponents.setUpperLimit(mEntity, upperLimit);
|
||||
|
||||
// Reset the limits
|
||||
resetLimits();
|
||||
|
|
@ -715,8 +787,8 @@ void HingeJoint::setMaxAngleLimit(decimal upperLimit) {
|
|||
void HingeJoint::resetLimits() {
|
||||
|
||||
// Reset the accumulated impulses for the limits
|
||||
mImpulseLowerLimit = 0.0;
|
||||
mImpulseUpperLimit = 0.0;
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
|
||||
// Wake up the two bodies of the joint
|
||||
awakeBodies();
|
||||
|
|
@ -725,9 +797,9 @@ void HingeJoint::resetLimits() {
|
|||
// Set the motor speed
|
||||
void HingeJoint::setMotorSpeed(decimal motorSpeed) {
|
||||
|
||||
if (motorSpeed != mMotorSpeed) {
|
||||
if (motorSpeed != mWorld.mHingeJointsComponents.getMotorSpeed(mEntity)) {
|
||||
|
||||
mMotorSpeed = motorSpeed;
|
||||
mWorld.mHingeJointsComponents.setMotorSpeed(mEntity, motorSpeed);
|
||||
|
||||
// Wake up the two bodies of the joint
|
||||
awakeBodies();
|
||||
|
|
@ -740,10 +812,12 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
|
|||
*/
|
||||
void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
|
||||
|
||||
if (maxMotorTorque != mMaxMotorTorque) {
|
||||
const decimal torque = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity);
|
||||
|
||||
assert(mMaxMotorTorque >= decimal(0.0));
|
||||
mMaxMotorTorque = maxMotorTorque;
|
||||
if (maxMotorTorque != torque) {
|
||||
|
||||
assert(torque >= decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setMaxMotorTorque(mEntity, maxMotorTorque);
|
||||
|
||||
// Wake up the two bodies of the joint
|
||||
awakeBodies();
|
||||
|
|
@ -771,8 +845,7 @@ decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
|
|||
// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
|
||||
// two angle limits in arguments.
|
||||
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
|
||||
decimal upperLimitAngle) const {
|
||||
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const {
|
||||
if (upperLimitAngle <= lowerLimitAngle) {
|
||||
return inputAngle;
|
||||
}
|
||||
|
|
@ -792,8 +865,7 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci
|
|||
}
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
|
||||
const Quaternion& orientationBody2) {
|
||||
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, const Quaternion& orientationBody2) {
|
||||
|
||||
decimal hingeAngle;
|
||||
|
||||
|
|
@ -802,7 +874,7 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
|
|||
currentOrientationDiff.normalize();
|
||||
|
||||
// Compute the relative rotation considering the initial orientation difference
|
||||
Quaternion relativeRotation = currentOrientationDiff * mInitOrientationDifferenceInv;
|
||||
Quaternion relativeRotation = currentOrientationDiff * mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity);
|
||||
relativeRotation.normalize();
|
||||
|
||||
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
|
||||
|
|
@ -816,7 +888,7 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
|
|||
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
|
||||
|
||||
// Compute the dot product of the relative rotation axis and the hinge axis
|
||||
decimal dotProduct = relativeRotation.getVectorV().dot(mA1);
|
||||
decimal dotProduct = relativeRotation.getVectorV().dot(mWorld.mHingeJointsComponents.getA1(mEntity));
|
||||
|
||||
// If the relative rotation axis and the hinge axis are pointing the same direction
|
||||
if (dotProduct >= decimal(0.0)) {
|
||||
|
|
@ -830,6 +902,84 @@ decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1,
|
|||
hingeAngle = computeNormalizedAngle(hingeAngle);
|
||||
|
||||
// Compute and return the corresponding angle near one the two limits
|
||||
return computeCorrespondingAngleNearLimits(hingeAngle, mLowerLimit, mUpperLimit);
|
||||
return computeCorrespondingAngleNearLimits(hingeAngle,
|
||||
mWorld.mHingeJointsComponents.getLowerLimit(mEntity),
|
||||
mWorld.mHingeJointsComponents.getUpperLimit(mEntity));
|
||||
}
|
||||
|
||||
// Return true if the limits of the joint are enabled
|
||||
/**
|
||||
* @return True if the limits of the joint are enabled and false otherwise
|
||||
*/
|
||||
bool HingeJoint::isLimitEnabled() const {
|
||||
return mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity);
|
||||
}
|
||||
|
||||
// Return true if the motor of the joint is enabled
|
||||
/**
|
||||
* @return True if the motor of joint is enabled and false otherwise
|
||||
*/
|
||||
bool HingeJoint::isMotorEnabled() const {
|
||||
return mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity);
|
||||
}
|
||||
|
||||
// Return the minimum angle limit
|
||||
/**
|
||||
* @return The minimum limit angle of the joint (in radian)
|
||||
*/
|
||||
decimal HingeJoint::getMinAngleLimit() const {
|
||||
return mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
}
|
||||
|
||||
// Return the maximum angle limit
|
||||
/**
|
||||
* @return The maximum limit angle of the joint (in radian)
|
||||
*/
|
||||
decimal HingeJoint::getMaxAngleLimit() const {
|
||||
return mWorld.mHingeJointsComponents.getUpperLimit(mEntity);
|
||||
}
|
||||
|
||||
// Return the motor speed
|
||||
/**
|
||||
* @return The current speed of the joint motor (in radian per second)
|
||||
*/
|
||||
decimal HingeJoint::getMotorSpeed() const {
|
||||
return mWorld.mHingeJointsComponents.getMotorSpeed(mEntity);
|
||||
}
|
||||
|
||||
// Return the maximum motor torque
|
||||
/**
|
||||
* @return The maximum torque of the joint motor (in Newtons)
|
||||
*/
|
||||
decimal HingeJoint::getMaxMotorTorque() const {
|
||||
return mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity);
|
||||
}
|
||||
|
||||
// Return the intensity of the current torque applied for the joint motor
|
||||
/**
|
||||
* @param timeStep The current time step (in seconds)
|
||||
* @return The intensity of the current torque (in Newtons) of the joint motor
|
||||
*/
|
||||
decimal HingeJoint::getMotorTorque(decimal timeStep) const {
|
||||
return mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) / timeStep;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the joint
|
||||
size_t HingeJoint::getSizeInBytes() const {
|
||||
return sizeof(HingeJoint);
|
||||
}
|
||||
|
||||
// Return a string representation
|
||||
std::string HingeJoint::to_string() const {
|
||||
return "HingeJoint{ lowerLimit=" + std::to_string(mWorld.mHingeJointsComponents.getLowerLimit(mEntity)) +
|
||||
", upperLimit=" + std::to_string(mWorld.mHingeJointsComponents.getUpperLimit(mEntity)) +
|
||||
"localAnchorPointBody1=" + mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + ", localAnchorPointBody2=" +
|
||||
mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", hingeLocalAxisBody1=" +
|
||||
mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity).to_string() +
|
||||
", hingeLocalAxisBody2=" + mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity).to_string() +
|
||||
", initOrientationDifferenceInv=" + mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() +
|
||||
", motorSpeed=" + std::to_string(mWorld.mHingeJointsComponents.getMotorSpeed(mEntity)) +
|
||||
", maxMotorTorque=" + std::to_string(mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity)) + ", isLimitEnabled=" +
|
||||
(mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) ? "true" : "false") + ", isMotorEnabled=" +
|
||||
(mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) ? "true" : "false") + "}";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -149,104 +149,6 @@ class HingeJoint : public Joint {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Hinge rotation axis (in local-space coordinates of body 1)
|
||||
Vector3 mHingeLocalAxisBody1;
|
||||
|
||||
/// Hinge rotation axis (in local-space coordiantes of body 2)
|
||||
Vector3 mHingeLocalAxisBody2;
|
||||
|
||||
/// Inertia tensor of body 1 (in world-space coordinates)
|
||||
Matrix3x3 mI1;
|
||||
|
||||
/// Inertia tensor of body 2 (in world-space coordinates)
|
||||
Matrix3x3 mI2;
|
||||
|
||||
/// Hinge rotation axis (in world-space coordinates) computed from body 1
|
||||
Vector3 mA1;
|
||||
|
||||
/// Vector from center of body 2 to anchor point in world-space
|
||||
Vector3 mR1World;
|
||||
|
||||
/// Vector from center of body 2 to anchor point in world-space
|
||||
Vector3 mR2World;
|
||||
|
||||
/// Cross product of vector b2 and a1
|
||||
Vector3 mB2CrossA1;
|
||||
|
||||
/// Cross product of vector c2 and a1;
|
||||
Vector3 mC2CrossA1;
|
||||
|
||||
/// Impulse for the 3 translation constraints
|
||||
Vector3 mImpulseTranslation;
|
||||
|
||||
/// Impulse for the 2 rotation constraints
|
||||
Vector2 mImpulseRotation;
|
||||
|
||||
/// 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 mass matrix K=JM^-1J^t for the 3 translation constraints
|
||||
Matrix3x3 mInverseMassMatrixTranslation;
|
||||
|
||||
/// Inverse mass matrix K=JM^-1J^t for the 2 rotation constraints
|
||||
Matrix2x2 mInverseMassMatrixRotation;
|
||||
|
||||
/// 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 vector for the error correction for the translation constraints
|
||||
Vector3 mBTranslation;
|
||||
|
||||
/// Bias vector for the error correction for the rotation constraints
|
||||
Vector2 mBRotation;
|
||||
|
||||
/// Bias of the lower limit constraint
|
||||
decimal mBLowerLimit;
|
||||
|
||||
/// Bias of the upper limit constraint
|
||||
decimal mBUpperLimit;
|
||||
|
||||
/// Inverse of the initial orientation difference between the bodies
|
||||
Quaternion mInitOrientationDifferenceInv;
|
||||
|
||||
/// 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 -------------------- //
|
||||
|
||||
|
|
@ -341,78 +243,6 @@ class HingeJoint : public Joint {
|
|||
virtual std::string to_string() const override;
|
||||
};
|
||||
|
||||
// Return true if the limits of the joint are enabled
|
||||
/**
|
||||
* @return True if the limits of the joint are enabled and false otherwise
|
||||
*/
|
||||
inline bool HingeJoint::isLimitEnabled() const {
|
||||
return mIsLimitEnabled;
|
||||
}
|
||||
|
||||
// Return true if the motor of the joint is enabled
|
||||
/**
|
||||
* @return True if the motor of joint is enabled and false otherwise
|
||||
*/
|
||||
inline bool HingeJoint::isMotorEnabled() const {
|
||||
return mIsMotorEnabled;
|
||||
}
|
||||
|
||||
// Return the minimum angle limit
|
||||
/**
|
||||
* @return The minimum limit angle of the joint (in radian)
|
||||
*/
|
||||
inline decimal HingeJoint::getMinAngleLimit() const {
|
||||
return mLowerLimit;
|
||||
}
|
||||
|
||||
// Return the maximum angle limit
|
||||
/**
|
||||
* @return The maximum limit angle of the joint (in radian)
|
||||
*/
|
||||
inline decimal HingeJoint::getMaxAngleLimit() const {
|
||||
return mUpperLimit;
|
||||
}
|
||||
|
||||
// Return the motor speed
|
||||
/**
|
||||
* @return The current speed of the joint motor (in radian per second)
|
||||
*/
|
||||
inline decimal HingeJoint::getMotorSpeed() const {
|
||||
return mMotorSpeed;
|
||||
}
|
||||
|
||||
// Return the maximum motor torque
|
||||
/**
|
||||
* @return The maximum torque of the joint motor (in Newtons)
|
||||
*/
|
||||
inline decimal HingeJoint::getMaxMotorTorque() const {
|
||||
return mMaxMotorTorque;
|
||||
}
|
||||
|
||||
// Return the intensity of the current torque applied for the joint motor
|
||||
/**
|
||||
* @param timeStep The current time step (in seconds)
|
||||
* @return The intensity of the current torque (in Newtons) of the joint motor
|
||||
*/
|
||||
inline decimal HingeJoint::getMotorTorque(decimal timeStep) const {
|
||||
return mImpulseMotor / timeStep;
|
||||
}
|
||||
|
||||
// Return the number of bytes used by the joint
|
||||
inline size_t HingeJoint::getSizeInBytes() const {
|
||||
return sizeof(HingeJoint);
|
||||
}
|
||||
|
||||
// Return a string representation
|
||||
inline std::string HingeJoint::to_string() const {
|
||||
return "HingeJoint{ lowerLimit=" + std::to_string(mLowerLimit) + ", upperLimit=" + std::to_string(mUpperLimit) +
|
||||
"localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() + ", localAnchorPointBody2=" +
|
||||
mLocalAnchorPointBody2.to_string() + ", hingeLocalAxisBody1=" + mHingeLocalAxisBody1.to_string() +
|
||||
", hingeLocalAxisBody2=" + mHingeLocalAxisBody2.to_string() + ", initOrientationDifferenceInv=" +
|
||||
mInitOrientationDifferenceInv.to_string() + ", motorSpeed=" + std::to_string(mMotorSpeed) +
|
||||
", maxMotorTorque=" + std::to_string(mMaxMotorTorque) + ", isLimitEnabled=" +
|
||||
(mIsLimitEnabled ? "true" : "false") + ", isMotorEnabled=" + (mIsMotorEnabled ? "true" : "false") + "}";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge
|
|||
mCollisionBodyComponents(mMemoryManager.getBaseAllocator()), mRigidBodyComponents(mMemoryManager.getBaseAllocator()),
|
||||
mTransformComponents(mMemoryManager.getBaseAllocator()), mProxyShapesComponents(mMemoryManager.getBaseAllocator()),
|
||||
mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()),
|
||||
mFixedJointsComponents(mMemoryManager.getBaseAllocator()),
|
||||
mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mHingeJointsComponents(mMemoryManager.getBaseAllocator()),
|
||||
mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager),
|
||||
mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr),
|
||||
mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr),
|
||||
|
|
@ -268,6 +268,9 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) {
|
|||
if (mFixedJointsComponents.hasComponent(jointEntity)) {
|
||||
mFixedJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
|
||||
}
|
||||
if (mHingeJointsComponents.hasComponent(jointEntity)) {
|
||||
mHingeJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
|
||||
}
|
||||
}
|
||||
|
||||
// Return true if two bodies overlap
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@
|
|||
#include "components/JointComponents.h"
|
||||
#include "components/BallAndSocketJointComponents.h"
|
||||
#include "components/FixedJointComponents.h"
|
||||
#include "components/HingeJointComponents.h"
|
||||
#include "collision/CollisionCallback.h"
|
||||
#include "collision/OverlapCallback.h"
|
||||
|
||||
|
|
@ -100,6 +101,9 @@ class CollisionWorld {
|
|||
/// Fixed joints Components
|
||||
FixedJointComponents mFixedJointsComponents;
|
||||
|
||||
/// Hinge joints Components
|
||||
HingeJointComponents mHingeJointsComponents;
|
||||
|
||||
/// Reference to the collision detection
|
||||
CollisionDetectionSystem mCollisionDetection;
|
||||
|
||||
|
|
|
|||
|
|
@ -358,10 +358,20 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
|
|||
// Hinge joint
|
||||
case JointType::HINGEJOINT:
|
||||
{
|
||||
const HingeJointInfo& info = static_cast<const HingeJointInfo&>(jointInfo);
|
||||
|
||||
// Create a HingeJoint component
|
||||
HingeJointComponents::HingeJointComponent hingeJointComponent(info.isLimitEnabled, info.isMotorEnabled,
|
||||
info.minAngleLimit, info.maxAngleLimit,
|
||||
info.motorSpeed, info.maxMotorTorque);
|
||||
mHingeJointsComponents.addComponent(entity, isJointDisabled, hingeJointComponent);
|
||||
|
||||
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
|
||||
sizeof(HingeJoint));
|
||||
const HingeJointInfo& info = static_cast<const HingeJointInfo&>(jointInfo);
|
||||
newJoint = new (allocatedMemory) HingeJoint(entity, *this, info);
|
||||
HingeJoint* joint = new (allocatedMemory) HingeJoint(entity, *this, info);
|
||||
|
||||
newJoint = joint;
|
||||
mHingeJointsComponents.setJoint(entity, joint);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user