Add FixedJointComponents class
This commit is contained in:
Daniel Chappuis
parent
8187c19fa3
commit
67d8411623
|
|
@ -148,6 +148,7 @@ SET (REACTPHYSICS3D_HEADERS
|
|||
"src/components/ProxyShapeComponents.h"
|
||||
"src/components/JointComponents.h"
|
||||
"src/components/BallAndSocketJointComponents.h"
|
||||
"src/components/FixedJointComponents.h"
|
||||
"src/collision/CollisionCallback.h"
|
||||
"src/collision/OverlapCallback.h"
|
||||
"src/mathematics/mathematics.h"
|
||||
|
|
@ -242,6 +243,7 @@ SET (REACTPHYSICS3D_SOURCES
|
|||
"src/components/ProxyShapeComponents.cpp"
|
||||
"src/components/JointComponents.cpp"
|
||||
"src/components/BallAndSocketJointComponents.cpp"
|
||||
"src/components/FixedJointComponents.cpp"
|
||||
"src/collision/CollisionCallback.cpp"
|
||||
"src/collision/OverlapCallback.cpp"
|
||||
"src/mathematics/mathematics_functions.cpp"
|
||||
|
|
|
|||
259
src/components/FixedJointComponents.cpp
Normal file
259
src/components/FixedJointComponents.cpp
Normal file
|
|
@ -0,0 +1,259 @@
|
|||
/********************************************************************************
|
||||
* 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 "FixedJointComponents.h"
|
||||
#include "engine/EntityManager.h"
|
||||
#include "mathematics/Matrix3x3.h"
|
||||
#include <cassert>
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
FixedJointComponents::FixedJointComponents(MemoryAllocator& allocator)
|
||||
:Components(allocator, sizeof(Entity) + sizeof(FixedJoint*) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion)) {
|
||||
|
||||
// Allocate memory for the components data
|
||||
allocate(INIT_NB_ALLOCATED_COMPONENTS);
|
||||
}
|
||||
|
||||
// Allocate memory for a given number of components
|
||||
void FixedJointComponents::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);
|
||||
FixedJoint** newJoints = reinterpret_cast<FixedJoint**>(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);
|
||||
Vector3* newImpulseRotation = reinterpret_cast<Vector3*>(newImpulseTranslation + nbComponentsToAllocate);
|
||||
Matrix3x3* newInverseMassMatrixTranslation = reinterpret_cast<Matrix3x3*>(newImpulseRotation + nbComponentsToAllocate);
|
||||
Matrix3x3* newInverseMassMatrixRotation = reinterpret_cast<Matrix3x3*>(newInverseMassMatrixTranslation + nbComponentsToAllocate);
|
||||
Vector3* newBiasTranslation = reinterpret_cast<Vector3*>(newInverseMassMatrixRotation + nbComponentsToAllocate);
|
||||
Vector3* newBiasRotation = reinterpret_cast<Vector3*>(newBiasTranslation + nbComponentsToAllocate);
|
||||
Quaternion* newInitOrientationDifferenceInv = reinterpret_cast<Quaternion*>(newBiasRotation + 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(FixedJoint*));
|
||||
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(Vector3));
|
||||
memcpy(newInverseMassMatrixTranslation, mInverseMassMatrixTranslation, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newInverseMassMatrixRotation, mInverseMassMatrixRotation, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newBiasTranslation, mBiasTranslation, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newBiasRotation, mBiasRotation, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newInitOrientationDifferenceInv, mInitOrientationDifferenceInv, mNbComponents * sizeof(Quaternion));
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Add a component
|
||||
void FixedJointComponents::addComponent(Entity jointEntity, bool isSleeping, const FixedJointComponent& 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);
|
||||
|
||||
// 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 FixedJointComponents::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(mImpulseRotation[srcIndex]);
|
||||
new (mImpulseRotation + destIndex) Vector3(mImpulseRotation[srcIndex]);
|
||||
new (mInverseMassMatrixTranslation + destIndex) Matrix3x3(mInverseMassMatrixTranslation[srcIndex]);
|
||||
new (mInverseMassMatrixRotation + destIndex) Matrix3x3(mInverseMassMatrixRotation[srcIndex]);
|
||||
new (mBiasTranslation + destIndex) Vector3(mBiasTranslation[srcIndex]);
|
||||
new (mBiasRotation + destIndex) Vector3(mBiasRotation[srcIndex]);
|
||||
new (mInitOrientationDifferenceInv + destIndex) Quaternion(mInitOrientationDifferenceInv[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 FixedJointComponents::swapComponents(uint32 index1, uint32 index2) {
|
||||
|
||||
// Copy component 1 data
|
||||
Entity jointEntity1(mJointEntities[index1]);
|
||||
FixedJoint* 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]);
|
||||
Vector3 impulseRotation1(mImpulseRotation[index1]);
|
||||
Matrix3x3 inverseMassMatrixTranslation1(mInverseMassMatrixTranslation[index1]);
|
||||
Matrix3x3 inverseMassMatrixRotation1(mInverseMassMatrixRotation[index1]);
|
||||
Vector3 biasTranslation1(mBiasTranslation[index1]);
|
||||
Vector3 biasRotation1(mBiasRotation[index1]);
|
||||
Quaternion initOrientationDifferenceInv1(mInitOrientationDifferenceInv[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) Vector3(impulseRotation1);
|
||||
new (mInverseMassMatrixTranslation + index2) Matrix3x3(inverseMassMatrixTranslation1);
|
||||
new (mInverseMassMatrixRotation + index2) Matrix3x3(inverseMassMatrixRotation1);
|
||||
new (mBiasTranslation + index2) Vector3(biasTranslation1);
|
||||
new (mBiasRotation + index2) Vector3(biasRotation1);
|
||||
new (mInitOrientationDifferenceInv + index2) Quaternion(initOrientationDifferenceInv1);
|
||||
|
||||
// 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 FixedJointComponents::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].~Vector3();
|
||||
mInverseMassMatrixTranslation[index].~Matrix3x3();
|
||||
mInverseMassMatrixRotation[index].~Matrix3x3();
|
||||
mBiasTranslation[index].~Vector3();
|
||||
mBiasRotation[index].~Vector3();
|
||||
mInitOrientationDifferenceInv[index].~Quaternion();
|
||||
}
|
||||
424
src/components/FixedJointComponents.h
Normal file
424
src/components/FixedJointComponents.h
Normal file
|
|
@ -0,0 +1,424 @@
|
|||
/********************************************************************************
|
||||
* 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_FIXED_JOINT_COMPONENTS_H
|
||||
#define REACTPHYSICS3D_FIXED_JOINT_COMPONENTS_H
|
||||
|
||||
// Libraries
|
||||
#include "mathematics/Transform.h"
|
||||
#include "mathematics/Matrix3x3.h"
|
||||
#include "engine/Entity.h"
|
||||
#include "components/Components.h"
|
||||
#include "containers/Map.h"
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Class declarations
|
||||
class MemoryAllocator;
|
||||
class EntityManager;
|
||||
class FixedJoint;
|
||||
enum class JointType;
|
||||
|
||||
// Class FixedJointComponents
|
||||
/**
|
||||
* This class represent the component of the ECS with data for the FixedJoint.
|
||||
*/
|
||||
class FixedJointComponents : public Components {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Array of joint entities
|
||||
Entity* mJointEntities;
|
||||
|
||||
/// Array of pointers to the joints
|
||||
FixedJoint** 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
|
||||
Vector3* 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)
|
||||
Matrix3x3* mInverseMassMatrixRotation;
|
||||
|
||||
/// Bias vector for the 3 translation constraints
|
||||
Vector3* mBiasTranslation;
|
||||
|
||||
/// Bias vector for the 3 rotation constraints
|
||||
Vector3* mBiasRotation;
|
||||
|
||||
/// Inverse of the initial orientation difference between the two bodies
|
||||
Quaternion* mInitOrientationDifferenceInv;
|
||||
|
||||
// -------------------- 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 FixedJointComponent {
|
||||
|
||||
/// Constructor
|
||||
FixedJointComponent() {
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
FixedJointComponents(MemoryAllocator& allocator);
|
||||
|
||||
/// Destructor
|
||||
virtual ~FixedJointComponents() override = default;
|
||||
|
||||
/// Add a component
|
||||
void addComponent(Entity jointEntity, bool isSleeping, const FixedJointComponent& component);
|
||||
|
||||
/// Return a pointer to a given joint
|
||||
FixedJoint* getJoint(Entity jointEntity) const;
|
||||
|
||||
/// Set the joint pointer to a given joint
|
||||
void setJoint(Entity jointEntity, FixedJoint* joint) const;
|
||||
|
||||
/// Return the local anchor point of body 1 for a given joint
|
||||
const Vector3& getLocalAnchoirPointBody1(Entity jointEntity) const;
|
||||
|
||||
/// Set the local anchor point of body 1 for a given joint
|
||||
void setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1);
|
||||
|
||||
/// Return the local anchor point of body 2 for a given joint
|
||||
const Vector3& getLocalAnchoirPointBody2(Entity jointEntity) const;
|
||||
|
||||
/// Set the local anchor point of body 2 for a given joint
|
||||
void setLocalAnchoirPointBody2(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
|
||||
Vector3& getImpulseRotation(Entity jointEntity);
|
||||
|
||||
/// Set the translation impulse
|
||||
void setImpulseRotation(Entity jointEntity, const Vector3& 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
|
||||
Matrix3x3& getInverseMassMatrixRotation(Entity jointEntity);
|
||||
|
||||
/// Set the rotation inverse mass matrix of the constraint
|
||||
void setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix);
|
||||
|
||||
/// Return the translation bias
|
||||
Vector3& getBiasTranslation(Entity jointEntity);
|
||||
|
||||
/// Set the translation impulse
|
||||
void setBiasTranslation(Entity jointEntity, const Vector3& impulseTranslation);
|
||||
|
||||
/// Return the rotation bias
|
||||
Vector3& getBiasRotation(Entity jointEntity);
|
||||
|
||||
/// Set the rotation impulse
|
||||
void setBiasRotation(Entity jointEntity, const Vector3 &impulseRotation);
|
||||
|
||||
/// Return the initial orientation difference
|
||||
Quaternion& getInitOrientationDifferenceInv(Entity jointEntity);
|
||||
|
||||
/// Set the rotation impulse
|
||||
void setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class BroadPhaseSystem;
|
||||
};
|
||||
|
||||
// Return a pointer to a given joint
|
||||
inline FixedJoint* FixedJointComponents::getJoint(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mJoints[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the joint pointer to a given joint
|
||||
inline void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* 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& FixedJointComponents::getLocalAnchoirPointBody1(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 FixedJointComponents::setLocalAnchoirPointBody1(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& FixedJointComponents::getLocalAnchoirPointBody2(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 FixedJointComponents::setLocalAnchoirPointBody2(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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::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& FixedJointComponents::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 FixedJointComponents::setI2(Entity jointEntity, const Matrix3x3& i2) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mI2[mMapEntityToComponentIndex[jointEntity]] = i2;
|
||||
}
|
||||
|
||||
// Return the translation impulse
|
||||
inline Vector3& FixedJointComponents::getImpulseTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void FixedJointComponents::setImpulseTranslation(Entity jointEntity, const Vector3& impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mImpulseTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the translation impulse
|
||||
inline Vector3& FixedJointComponents::getImpulseRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mImpulseRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void FixedJointComponents::setImpulseRotation(Entity jointEntity, const Vector3& impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mImpulseRotation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the translation inverse mass matrix of the constraint
|
||||
inline Matrix3x3& FixedJointComponents::getInverseMassMatrixTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
|
||||
// Set the translation inverse mass matrix of the constraint
|
||||
inline void FixedJointComponents::setInverseMassMatrixTranslation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInverseMassMatrixTranslation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
|
||||
}
|
||||
|
||||
// Return the rotation inverse mass matrix of the constraint
|
||||
inline Matrix3x3& FixedJointComponents::getInverseMassMatrixRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation inverse mass matrix of the constraint
|
||||
inline void FixedJointComponents::setInverseMassMatrixRotation(Entity jointEntity, const Matrix3x3& inverseMassMatrix) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInverseMassMatrixRotation[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
|
||||
}
|
||||
|
||||
// Return the translation bias
|
||||
inline Vector3& FixedJointComponents::getBiasTranslation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mBiasTranslation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the translation impulse
|
||||
inline void FixedJointComponents::setBiasTranslation(Entity jointEntity, const Vector3 &impulseTranslation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mBiasTranslation[mMapEntityToComponentIndex[jointEntity]] = impulseTranslation;
|
||||
}
|
||||
|
||||
// Return the rotation bias
|
||||
inline Vector3& FixedJointComponents::getBiasRotation(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mBiasRotation[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation impulse
|
||||
inline void FixedJointComponents::setBiasRotation(Entity jointEntity, const Vector3& impulseRotation) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mBiasRotation[mMapEntityToComponentIndex[jointEntity]] = impulseRotation;
|
||||
}
|
||||
|
||||
// Return the initial orientation difference
|
||||
inline Quaternion& FixedJointComponents::getInitOrientationDifferenceInv(Entity jointEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the rotation impulse
|
||||
inline void FixedJointComponents::setInitOrientationDifferenceInv(Entity jointEntity, const Quaternion& initOrientationDifferenceInv) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInitOrientationDifferenceInv[mMapEntityToComponentIndex[jointEntity]] = initOrientationDifferenceInv;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -36,14 +36,14 @@ const decimal FixedJoint::BETA = decimal(0.2);
|
|||
|
||||
// Constructor
|
||||
FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo)
|
||||
: Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
|
||||
: Joint(entity, world, jointInfo) {
|
||||
|
||||
// Compute the local-space anchor point for each body
|
||||
const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||
|
||||
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
|
||||
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||
|
||||
// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
|
||||
//
|
||||
|
|
@ -56,7 +56,7 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo
|
|||
// q20 = initial orientation of body 2
|
||||
// q10 = initial orientation of body 1
|
||||
// r0 = initial rotation rotation from body 1 to body 2
|
||||
mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation();
|
||||
mWorld.mFixedJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation());
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
|
|
@ -77,16 +77,22 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
const Quaternion& orientationBody2 = body2->getTransform().getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mI1 = body1->getInertiaTensorInverseWorld();
|
||||
mI2 = body2->getInertiaTensorInverseWorld();
|
||||
mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mFixedJointsComponents.setI1(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = orientationBody1 * mLocalAnchorPointBody1;
|
||||
mR2World = orientationBody2 * mLocalAnchorPointBody2;
|
||||
mWorld.mFixedJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity));
|
||||
mWorld.mFixedJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity));
|
||||
|
||||
// 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);
|
||||
|
||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
|
||||
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
|
||||
|
|
@ -95,45 +101,52 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
|
|||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
|
||||
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
|
||||
|
||||
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
|
||||
mInverseMassMatrixTranslation.setToZero();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the constraint for the 3 translation constraints
|
||||
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||
mBiasTranslation.setToZero();
|
||||
const decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||
Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity);
|
||||
biasTranslation.setToZero();
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
|
||||
mWorld.mFixedJointsComponents.setBiasTranslation(mEntity, biasFactor * (x2 + r2World - x1 - r1World));
|
||||
}
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mInverseMassMatrixRotation = mI1 + mI2;
|
||||
Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation = i1 + i2;
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
|
||||
mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" for the 3 rotation constraints
|
||||
mBiasRotation.setToZero();
|
||||
Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity);
|
||||
biasRotation.setToZero();
|
||||
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
|
||||
mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV();
|
||||
const Quaternion qError = orientationBody2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse();
|
||||
mWorld.mFixedJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV());
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!constraintSolverData.isWarmStartingActive) {
|
||||
|
||||
Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity);
|
||||
Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity);
|
||||
|
||||
// Reset the accumulated impulses
|
||||
mImpulseTranslation.setToZero();
|
||||
mImpulseRotation.setToZero();
|
||||
impulseTranslation.setToZero();
|
||||
impulseRotation.setToZero();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -157,26 +170,35 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
|||
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity);
|
||||
const Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity);
|
||||
|
||||
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
|
||||
Vector3 linearImpulseBody1 = -mImpulseTranslation;
|
||||
Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
|
||||
Vector3 linearImpulseBody1 = -impulseTranslation;
|
||||
Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
|
||||
angularImpulseBody1 += -mImpulseRotation;
|
||||
angularImpulseBody1 += -impulseRotation;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
|
||||
Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
|
||||
Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
|
||||
angularImpulseBody2 += mImpulseRotation;
|
||||
angularImpulseBody2 += impulseRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * mImpulseTranslation;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
v2 += inverseMassBody2 * impulseTranslation;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
|
|
@ -199,48 +221,59 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
|
|||
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 3 translation constraints
|
||||
const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
const Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity);
|
||||
const Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambda = mInverseMassMatrixTranslation *
|
||||
(-JvTranslation - mBiasTranslation);
|
||||
mImpulseTranslation += deltaLambda;
|
||||
const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - biasTranslation);
|
||||
mWorld.mFixedJointsComponents.setImpulseTranslation(mEntity, mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) + deltaLambda);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambda;
|
||||
Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
|
||||
Vector3 angularImpulseBody1 = deltaLambda.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 for body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
|
||||
const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World);
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * deltaLambda;
|
||||
w2 += mI2 * angularImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
const Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity);
|
||||
const Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
|
||||
mImpulseRotation += deltaLambda2;
|
||||
Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation);
|
||||
mWorld.mFixedJointsComponents.setImpulseRotation(mEntity, mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) + deltaLambda2);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
|
||||
angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mI1 * angularImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mI2 * deltaLambda2;
|
||||
w2 += i2 * deltaLambda2;
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
|
|
@ -268,17 +301,23 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mI1 = body1->getInertiaTensorInverseWorld();
|
||||
mI2 = body2->getInertiaTensorInverseWorld();
|
||||
mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mFixedJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mR1World = q1 * mLocalAnchorPointBody1;
|
||||
mR2World = q2 * mLocalAnchorPointBody2;
|
||||
mWorld.mFixedJointsComponents.setR1World(mEntity, q1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity));
|
||||
mWorld.mFixedJointsComponents.setR2World(mEntity, q2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity));
|
||||
|
||||
// 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 --------------- //
|
||||
|
||||
|
|
@ -287,27 +326,28 @@ void FixedJoint::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.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
|
@ -315,11 +355,11 @@ void FixedJoint::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;
|
||||
|
|
@ -330,10 +370,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mInverseMassMatrixRotation = mI1 + mI2;
|
||||
Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation = i1 + i2;
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
|
||||
mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, inverseMassMatrixRotation.getInverse());
|
||||
}
|
||||
|
||||
// Calculate difference in rotation
|
||||
|
|
@ -351,7 +392,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
// q1 = current rotation of body 1
|
||||
// q2 = current rotation of body 2
|
||||
// qError = error that needs to be reduced to zero
|
||||
Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse();
|
||||
Quaternion qError = q2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * q1.getInverse();
|
||||
|
||||
// A quaternion can be seen as:
|
||||
//
|
||||
|
|
@ -365,20 +406,20 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 lambdaRotation = mInverseMassMatrixRotation * (-errorRotation);
|
||||
Vector3 lambdaRotation = inverseMassMatrixRotation * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// 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 pseudo velocity of body 2
|
||||
w2 = mI2 * lambdaRotation;
|
||||
w2 = i2 * lambdaRotation;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
|
|
@ -390,3 +431,11 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
|
|||
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
|
||||
}
|
||||
|
||||
// Return a string representation
|
||||
std::string FixedJoint::to_string() const {
|
||||
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() +
|
||||
", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() +
|
||||
", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() +
|
||||
"}";
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -73,47 +73,6 @@ class FixedJoint : public Joint {
|
|||
// Beta value for the bias factor of position correction
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- 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;
|
||||
|
||||
/// 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
|
||||
Vector3 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)
|
||||
Matrix3x3 mInverseMassMatrixRotation;
|
||||
|
||||
/// Bias vector for the 3 translation constraints
|
||||
Vector3 mBiasTranslation;
|
||||
|
||||
/// Bias vector for the 3 rotation constraints
|
||||
Vector3 mBiasRotation;
|
||||
|
||||
/// Inverse of the initial orientation difference between the two bodies
|
||||
Quaternion mInitOrientationDifferenceInv;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Return the number of bytes used by the joint
|
||||
|
|
@ -156,14 +115,6 @@ inline size_t FixedJoint::getSizeInBytes() const {
|
|||
return sizeof(FixedJoint);
|
||||
}
|
||||
|
||||
// Return a string representation
|
||||
inline std::string FixedJoint::to_string() const {
|
||||
return "FixedJoint{ localAnchorPointBody1=" + mLocalAnchorPointBody1.to_string() +
|
||||
", localAnchorPointBody2=" + mLocalAnchorPointBody2.to_string() +
|
||||
", initOrientationDifferenceInv=" + mInitOrientationDifferenceInv.to_string() +
|
||||
"}";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -41,6 +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()),
|
||||
mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager),
|
||||
mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr),
|
||||
mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr),
|
||||
|
|
@ -261,6 +262,12 @@ void CollisionWorld::setJointDisabled(Entity jointEntity, bool isDisabled) {
|
|||
|
||||
// TODO : Make sure we notify all the components here ...
|
||||
mJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
|
||||
if (mBallAndSocketJointsComponents.hasComponent(jointEntity)) {
|
||||
mBallAndSocketJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
|
||||
}
|
||||
if (mFixedJointsComponents.hasComponent(jointEntity)) {
|
||||
mFixedJointsComponents.setIsEntityDisabled(jointEntity, isDisabled);
|
||||
}
|
||||
}
|
||||
|
||||
// Return true if two bodies overlap
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
#include "components/ProxyShapeComponents.h"
|
||||
#include "components/JointComponents.h"
|
||||
#include "components/BallAndSocketJointComponents.h"
|
||||
#include "components/FixedJointComponents.h"
|
||||
#include "collision/CollisionCallback.h"
|
||||
#include "collision/OverlapCallback.h"
|
||||
|
||||
|
|
@ -96,6 +97,9 @@ class CollisionWorld {
|
|||
/// Ball And Socket joints Components
|
||||
BallAndSocketJointComponents mBallAndSocketJointsComponents;
|
||||
|
||||
/// Fixed joints Components
|
||||
FixedJointComponents mFixedJointsComponents;
|
||||
|
||||
/// Reference to the collision detection
|
||||
CollisionDetectionSystem mCollisionDetection;
|
||||
|
||||
|
|
|
|||
|
|
@ -368,10 +368,19 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
|
|||
// Fixed joint
|
||||
case JointType::FIXEDJOINT:
|
||||
{
|
||||
// Create a BallAndSocketJoint component
|
||||
FixedJointComponents::FixedJointComponent fixedJointComponent;
|
||||
mFixedJointsComponents.addComponent(entity, isJointDisabled, fixedJointComponent);
|
||||
|
||||
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
|
||||
sizeof(FixedJoint));
|
||||
const FixedJointInfo& info = static_cast<const FixedJointInfo&>(jointInfo);
|
||||
newJoint = new (allocatedMemory) FixedJoint(entity, *this, info);
|
||||
FixedJoint* joint = new (allocatedMemory) FixedJoint(entity, *this, info);
|
||||
|
||||
newJoint = joint;
|
||||
|
||||
mFixedJointsComponents.setJoint(entity, joint);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -467,6 +476,9 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
|
|||
sizeof(JointListElement));
|
||||
JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint,
|
||||
body1->mJointsList);
|
||||
RigidBody* test1 = joint->getBody1();
|
||||
RigidBody* test2 = joint->getBody2();
|
||||
|
||||
body1->mJointsList = jointListElement1;
|
||||
|
||||
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user