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

Working on joints components

Browse Source
This commit is contained in:
Daniel Chappuis 2019-08-19 07:23:19 +02:00
parent 2144b8e571
commit 0230b74462
16 changed files with 403 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/body/RigidBody.cpp
View File

@ -781,7 +781,6 @@ void RigidBody::setIsSleeping(bool isSleeping) {
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set isSleeping=" +
(isSleeping ? "true" : "false"));
}
// Set whether or not the body is allowed to go to sleep

3
src/body/RigidBody.h
View File

@ -84,7 +84,7 @@ class RigidBody : public CollisionBody {
void updateInertiaTensorInverseWorld();
/// Set the variable to know whether or not the body is sleeping
void setIsSleeping(bool isSleeping);
void setIsSleeping(bool isSleeping);
public :
@ -222,6 +222,7 @@ class RigidBody : public CollisionBody {
friend class DynamicsWorld;
friend class ContactSolverSystem;
friend class Joint;
friend class BallAndSocketJoint;
friend class SliderJoint;
friend class HingeJoint;

11
src/components/JointComponents.cpp
View File

@ -33,7 +33,8 @@ using namespace reactphysics3d;
// Constructor
JointComponents::JointComponents(MemoryAllocator& allocator)
:Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity)) {
:Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) +
sizeof(Joint*)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -55,6 +56,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) {
Entity* newJointsEntities = static_cast<Entity*>(newBuffer);
Entity* newBody1Entities = reinterpret_cast<Entity*>(newJointsEntities + nbComponentsToAllocate);
Entity* newBody2Entities = reinterpret_cast<Entity*>(newBody1Entities + nbComponentsToAllocate);
Joint** newJoints = reinterpret_cast<Joint**>(newBody2Entities + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@ -63,6 +65,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newJointsEntities, mJointEntities, mNbComponents * sizeof(Entity));
memcpy(newBody1Entities, mBody1Entities, mNbComponents * sizeof(Entity));
memcpy(newBody2Entities, mBody2Entities, mNbComponents * sizeof(Entity));
memcpy(newJoints, mJoints, mNbComponents * sizeof(Joint*));
// Deallocate previous memory
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@ -73,6 +76,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) {
mJointEntities = newJointsEntities;
mBody1Entities = newBody1Entities;
mBody2Entities = newBody2Entities;
mJoints = newJoints;
}
// Add a component
@ -85,6 +89,7 @@ void JointComponents::addComponent(Entity jointEntity, bool isSleeping, const Jo
new (mJointEntities + index) Entity(jointEntity);
new (mBody1Entities + index) Entity(component.body1Entity);
new (mBody2Entities + index) Entity(component.body2Entity);
mJoints[index] = component.joint;
// Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
@ -105,6 +110,7 @@ void JointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]);
new (mBody1Entities + destIndex) Entity(mBody1Entities[srcIndex]);
new (mBody2Entities + destIndex) Entity(mBody2Entities[srcIndex]);
mJoints[destIndex] = mJoints[srcIndex];
// Destroy the source component
destroyComponent(srcIndex);
@ -124,6 +130,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) {
Entity jointEntity1(mJointEntities[index1]);
Entity body1Entity1(mBody1Entities[index1]);
Entity body2Entity1(mBody2Entities[index1]);
Joint* joint1 = mJoints[index1];
// Destroy component 1
destroyComponent(index1);
@ -134,6 +141,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) {
new (mJointEntities + index2) Entity(jointEntity1);
new (mBody1Entities + index2) Entity(body1Entity1);
new (mBody2Entities + index2) Entity(body2Entity1);
mJoints[index2] = joint1;
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));
@ -155,4 +163,5 @@ void JointComponents::destroyComponent(uint32 index) {
mJointEntities[index].~Entity();
mBody1Entities[index].~Entity();
mBody2Entities[index].~Entity();
mJoints[index] = nullptr;
}

18
src/components/JointComponents.h
View File

@ -38,6 +38,7 @@ namespace reactphysics3d {
// Class declarations
class MemoryAllocator;
class EntityManager;
class Joint;
// Class JointComponents
/**
@ -59,6 +60,9 @@ class JointComponents : public Components {
/// Array of body entities of the first bodies of the joints
Entity* mBody2Entities;
/// Array with pointers to the joints
Joint** mJoints;
// -------------------- Methods -------------------- //
/// Allocate memory for a given number of components
@ -80,10 +84,11 @@ class JointComponents : public Components {
const Entity body1Entity;
const Entity body2Entity;
Joint* joint;
/// Constructor
JointComponent(Entity body1Entity, Entity body2Entity)
: body1Entity(body1Entity), body2Entity(body2Entity) {
JointComponent(Entity body1Entity, Entity body2Entity, Joint* joint)
: body1Entity(body1Entity), body2Entity(body2Entity), joint(joint) {
}
};
@ -105,6 +110,9 @@ class JointComponents : public Components {
/// Return the entity of the second body of a joint
Entity getBody2Entity(Entity jointEntity) const;
/// Return a pointer to the joint
Joint* getJoint(Entity jointEntity) const;
// -------------------- Friendship -------------------- //
friend class BroadPhaseSystem;
@ -122,6 +130,12 @@ inline Entity JointComponents::getBody2Entity(Entity jointEntity) const {
return mBody2Entities[mMapEntityToComponentIndex[jointEntity]];
}
// Return a pointer to the joint
inline Joint* JointComponents::getJoint(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mJoints[mMapEntityToComponentIndex[jointEntity]];
}
}
#endif

96
src/constraint/BallAndSocketJoint.cpp
View File

@ -27,6 +27,7 @@
#include "BallAndSocketJoint.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d;
@ -34,26 +35,38 @@ using namespace reactphysics3d;
const decimal BallAndSocketJoint::BETA = decimal(0.2);
// Constructor
BallAndSocketJoint::BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo)
: Joint(entity, jointInfo), mImpulse(Vector3(0, 0, 0)) {
BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulse(Vector3(0, 0, 0)) {
// Get the transforms of the two bodies
Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
// Compute the local-space anchor point for each body
mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody1 = body1Transform.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = body2Transform.getInverse() * jointInfo.anchorPointWorldSpace;
}
// Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies center of mass and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -64,8 +77,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity());
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
@ -75,7 +88,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Compute the inverse mass matrix K^-1
mInverseMassMatrix.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
}
@ -97,8 +111,11 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -111,22 +128,25 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body 1
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body to the body 2
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * mImpulse;
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * mImpulse;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -146,37 +166,44 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body 2
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * deltaLambda;
v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * deltaLambda;
w2 += mI2 * angularImpulseBody2;
}
// Solve the position constraint (for position error correction)
void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies center of mass and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity);
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
@ -194,7 +221,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrix.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrix = massMatrix.getInverse();
}
@ -231,9 +259,9 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}

2
src/constraint/BallAndSocketJoint.h
View File

@ -125,7 +125,7 @@ class BallAndSocketJoint : public Joint {
// -------------------- Methods -------------------- //
/// Constructor
BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo);
BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo);
/// Destructor
virtual ~BallAndSocketJoint() override = default;

102
src/constraint/FixedJoint.cpp
View File

@ -27,6 +27,7 @@
#include "FixedJoint.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d;
@ -34,12 +35,13 @@ using namespace reactphysics3d;
const decimal FixedJoint::BETA = decimal(0.2);
// Constructor
FixedJoint::FixedJoint(Entity entity, const FixedJointInfo& jointInfo)
: Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform();
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;
@ -60,15 +62,23 @@ FixedJoint::FixedJoint(Entity entity, const FixedJointInfo& jointInfo)
// Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -79,8 +89,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity());
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity());
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
@ -90,7 +100,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -104,7 +115,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotation = mI1 + mI2;
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
@ -128,8 +140,12 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -138,8 +154,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -166,8 +182,12 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -176,8 +196,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// --------------- Translation Constraints --------------- //
@ -226,23 +246,31 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
// Solve the position constraint (for position error correction)
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity);
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
@ -262,7 +290,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -302,7 +331,8 @@ 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;
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
@ -354,9 +384,9 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}

2
src/constraint/FixedJoint.h
View File

@ -136,7 +136,7 @@ class FixedJoint : public Joint {
// -------------------- Methods -------------------- //
/// Constructor
FixedJoint(Entity entity, const FixedJointInfo& jointInfo);
FixedJoint(Entity entity, DynamicsWorld& world, const FixedJointInfo& jointInfo);
/// Destructor
virtual ~FixedJoint() override = default;

133
src/constraint/HingeJoint.cpp
View File

@ -27,6 +27,7 @@
#include "HingeJoint.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d;
@ -34,8 +35,8 @@ using namespace reactphysics3d;
const decimal HingeJoint::BETA = decimal(0.2);
// Constructor
HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo)
: Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0),
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),
@ -46,8 +47,8 @@ HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo)
assert(mUpperLimit >= decimal(0) && mUpperLimit <= decimal(2.0) * PI);
// Compute the local-space anchor point for each body
Transform transform1 = mBody1->getTransform();
Transform transform2 = mBody2->getTransform();
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;
@ -67,15 +68,23 @@ HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo)
// Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = orientationBody1 * mLocalAnchorPointBody1;
@ -113,8 +122,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity());
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
@ -122,7 +131,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -148,7 +158,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
@ -197,8 +208,12 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -207,8 +222,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@ -256,8 +271,12 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -266,8 +285,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// --------------- Translation Constraints --------------- //
@ -409,19 +428,27 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// do not execute this method
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity);
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space
mR1World = q1 * mLocalAnchorPointBody1;
@ -453,8 +480,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
@ -462,7 +489,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose();
mInverseMassMatrixTranslation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslation = massMatrix.getInverse();
}
@ -514,7 +542,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mC2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mInverseMassMatrixRotation.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotation = matrixKRotation.getInverse();
}
@ -611,10 +640,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
}
}
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}
@ -645,8 +674,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
mImpulseMotor = 0.0;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
// Set the minimum angle limit
@ -655,7 +683,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
*/
void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI);
assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0 * PI));
if (lowerLimit != mLowerLimit) {
@ -672,7 +700,7 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) {
*/
void HingeJoint::setMaxAngleLimit(decimal upperLimit) {
assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0 * PI));
if (upperLimit != mUpperLimit) {
@ -691,8 +719,7 @@ void HingeJoint::resetLimits() {
mImpulseUpperLimit = 0.0;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
// Set the motor speed
@ -703,8 +730,7 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
mMotorSpeed = motorSpeed;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
}
@ -716,12 +742,11 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
if (maxMotorTorque != mMaxMotorTorque) {
assert(mMaxMotorTorque >= 0.0);
assert(mMaxMotorTorque >= decimal(0.0));
mMaxMotorTorque = maxMotorTorque;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
}
@ -729,7 +754,7 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
// Convert it into the range [-2*pi; 2*pi]
angle = fmod(angle, PI_TIMES_2);
angle = std::fmod(angle, PI_TIMES_2);
// Convert it into the range [-pi; pi]
if (angle < -PI) {
@ -752,13 +777,13 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci
return inputAngle;
}
else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
}
else if (inputAngle < lowerLimitAngle) {
decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
}
else {

2
src/constraint/HingeJoint.h
View File

@ -287,7 +287,7 @@ class HingeJoint : public Joint {
// -------------------- Methods -------------------- //
/// Constructor
HingeJoint(Entity entity, const HingeJointInfo& jointInfo);
HingeJoint(Entity entity, DynamicsWorld& world, const HingeJointInfo& jointInfo);
/// Destructor
virtual ~HingeJoint() override = default;

41
src/constraint/Joint.cpp
View File

@ -25,16 +25,47 @@
// Libraries
#include "Joint.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d;
// Constructor
Joint::Joint(Entity entity, const JointInfo& jointInfo)
:mEntity(entity), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()),
mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type),
Joint::Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo)
:mEntity(entity), mWorld(world), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
assert(mBody1 != nullptr);
assert(mBody2 != nullptr);
}
// Return the reference to the body 1
/**
* @return The first body involved in the joint
*/
RigidBody* Joint::getBody1() const {
const Entity body1Entiy = mWorld.mJointsComponents.getBody1Entity(mEntity);
return mWorld.mRigidBodyComponents.getRigidBody(body1Entiy);
}
// Return the reference to the body 2
/**
* @return The second body involved in the joint
*/
RigidBody* Joint::getBody2() const {
const Entity body2Entiy = mWorld.mJointsComponents.getBody2Entity(mEntity);
return mWorld.mRigidBodyComponents.getRigidBody(body2Entiy);
}
// Awake the two bodies of the joint
void Joint::awakeBodies() const {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Wake up the two bodies of the joint
body1->setIsSleeping(false);
body2->setIsSleeping(false);
}

36
src/constraint/Joint.h
View File

@ -123,19 +123,8 @@ class Joint {
/// Entity ID of the joint
Entity mEntity;
/// Pointer to the first body of the joint
// TODO : Use Entities instead
RigidBody* const mBody1;
/// Pointer to the second body of the joint
// TODO : Use Entities instead
RigidBody* const mBody2;
/// Entity of the first body of the joint
Entity mBody1Entity;
/// Entity of the second body of the joint
Entity mBody2Entity;
/// Reference to the physics world
DynamicsWorld& mWorld;
/// Type of the joint
const JointType mType;
@ -172,12 +161,15 @@ class Joint {
/// Solve the position constraint
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
/// Awake the two bodies of the joint
void awakeBodies() const;
public :
// -------------------- Methods -------------------- //
/// Constructor
Joint(Entity entity, const JointInfo& jointInfo);
Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo);
/// Destructor
virtual ~Joint() = default;
@ -213,22 +205,6 @@ class Joint {
friend class ConstraintSolverSystem;
};
// Return the reference to the body 1
/**
* @return The first body involved in the joint
*/
inline RigidBody* Joint::getBody1() const {
return mBody1;
}
// Return the reference to the body 2
/**
* @return The second body involved in the joint
*/
inline RigidBody* Joint::getBody2() const {
return mBody2;
}
// Return the type of the joint
/**
* @return The type of the joint

146
src/constraint/SliderJoint.cpp
View File

@ -27,6 +27,7 @@
#include "SliderJoint.h"
#include "systems/ConstraintSolverSystem.h"
#include "components/RigidBodyComponents.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d;
@ -34,8 +35,8 @@ using namespace reactphysics3d;
const decimal SliderJoint::BETA = decimal(0.2);
// Constructor
SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo)
: Joint(entity, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo)
: Joint(entity, world, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
mLowerLimit(jointInfo.minTranslationLimit),
@ -48,8 +49,8 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo)
assert(mMaxMotorForce >= decimal(0.0));
// Compute the local-space anchor point for each body
const Transform& transform1 = mBody1->getTransform();
const Transform& transform2 = mBody2->getTransform();
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;
@ -64,10 +65,12 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo)
// q20 = initial orientation of body 2
// q10 = initial orientation of body 1
// r0 = initial rotation rotation from body 1 to body 2
// TODO : Do not compute the inverse here, it has already been computed above
mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Compute the slider axis in local-space of body 1
mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
// TODO : Do not compute the inverse here, it has already been computed above
mSliderAxisBody1 = transform1.getOrientation().getInverse() *
jointInfo.sliderAxisWorldSpace;
mSliderAxisBody1.normalize();
}
@ -75,15 +78,23 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo)
// Initialize before solving the constraint
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
// Get the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point
mR1 = orientationBody1 * mLocalAnchorPointBody1;
@ -124,8 +135,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity());
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity());
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
@ -141,7 +152,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
@ -157,7 +170,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2;
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
@ -215,8 +230,12 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -225,8 +244,8 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
@ -277,8 +296,12 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity);
// Get the bodies entities
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
@ -287,8 +310,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// --------------- Translation Constraints --------------- //
@ -441,19 +464,27 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// do not execute this method
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity);
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
mI2 = mBody2->getInertiaTensorInverseWorld();
mI1 = body1->getInertiaTensorInverseWorld();
mI2 = body2->getInertiaTensorInverseWorld();
// Vector from body center to the anchor point
mR1 = q1 * mLocalAnchorPointBody1;
@ -488,8 +519,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
@ -505,7 +536,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mR2CrossN2.dot(I2R2CrossN2);
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
mInverseMassMatrixTranslationConstraint.setToZero();
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
}
@ -548,7 +581,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mInverseMassMatrixRotationConstraint = mI1 + mI2;
if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) {
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
@ -610,8 +645,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity);
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
mInverseMassMatrixLimit = body1MassInverse + body2MassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
@ -686,10 +721,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
}
}
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}
// Enable/Disable the limits of the joint
@ -719,8 +754,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
mImpulseMotor = 0.0;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
// Return the current translation value of the joint
@ -731,11 +765,18 @@ decimal SliderJoint::getTranslation() const {
// TODO : Check if we need to compare rigid body position or center of mass here
// Get the bodies entities
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Quaternion& q1 = mBody1->getTransform().getOrientation();
const Quaternion& q2 = mBody2->getTransform().getOrientation();
const Transform& transform1 = mWorld.mTransformComponents.getTransform(body1Entity);
const Transform& transform2 = mWorld.mTransformComponents.getTransform(body2Entity);
const Vector3& x1 = transform1.getPosition();
const Vector3& x2 = transform2.getPosition();
const Quaternion& q1 = transform1.getOrientation();
const Quaternion& q2 = transform2.getOrientation();
// Compute the two anchor points in world-space coordinates
const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1;
@ -794,8 +835,7 @@ void SliderJoint::resetLimits() {
mImpulseUpperLimit = 0.0;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
// Set the motor speed
@ -809,8 +849,7 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
mMotorSpeed = motorSpeed;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
}
@ -822,11 +861,10 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
if (maxMotorForce != mMaxMotorForce) {
assert(mMaxMotorForce >= 0.0);
assert(mMaxMotorForce >= decimal(0.0));
mMaxMotorForce = maxMotorForce;
// Wake up the two bodies of the joint
mBody1->setIsSleeping(false);
mBody2->setIsSleeping(false);
awakeBodies();
}
}

2
src/constraint/SliderJoint.h
View File

@ -289,7 +289,7 @@ class SliderJoint : public Joint {
// -------------------- Methods -------------------- //
/// Constructor
SliderJoint(Entity entity, const SliderJointInfo& jointInfo);
SliderJoint(Entity entity, DynamicsWorld& world, const SliderJointInfo& jointInfo);
/// Destructor
virtual ~SliderJoint() override = default;

44
src/engine/DynamicsWorld.cpp
View File

@ -322,11 +322,6 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
Joint* newJoint = nullptr;
bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) &&
mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity());
JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity());
mJointsComponents.addComponent(entity, isJointDisabled, jointComponent);
// Allocate memory to create the new joint
switch(jointInfo.type) {
@ -337,7 +332,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = static_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(entity, info);
newJoint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info);
break;
}
@ -347,7 +342,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(SliderJoint));
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(entity, info);
newJoint = new (allocatedMemory) SliderJoint(entity, *this, info);
break;
}
@ -357,7 +352,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(HingeJoint));
const HingeJointInfo& info = static_cast<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(entity, info);
newJoint = new (allocatedMemory) HingeJoint(entity, *this, info);
break;
}
@ -367,7 +362,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(FixedJoint));
const FixedJointInfo& info = static_cast<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(entity, info);
newJoint = new (allocatedMemory) FixedJoint(entity, *this, info);
break;
}
@ -378,6 +373,11 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
}
}
bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) &&
mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity());
JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint);
mJointsComponents.addComponent(entity, isJointDisabled, jointComponent);
// If the collision between the two bodies of the constraint is disabled
if (!jointInfo.isCollisionEnabled) {
@ -418,16 +418,19 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
}
RigidBody* body1 = joint->getBody1();
RigidBody* body2 = joint->getBody2();
// Wake up the two bodies of the joint
joint->getBody1()->setIsSleeping(false);
joint->getBody2()->setIsSleeping(false);
body1->setIsSleeping(false);
body2->setIsSleeping(false);
// Remove the joint from the world
mJoints.remove(joint);
// Remove the joint from the joint list of the bodies involved in the joint
joint->mBody1->removeJointFromJointsList(mMemoryManager, joint);
joint->mBody2->removeJointFromJointsList(mMemoryManager, joint);
body1->removeJointFromJointsList(mMemoryManager, joint);
body2->removeJointFromJointsList(mMemoryManager, joint);
size_t nbBytes = joint->getSizeInBytes();
@ -448,26 +451,29 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
assert(joint != nullptr);
RigidBody* body1 = joint->getBody1();
RigidBody* body2 = joint->getBody2();
// Add the joint at the beginning of the linked list of joints of the first body
void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(JointListElement));
JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint,
joint->mBody1->mJointsList);
joint->mBody1->mJointsList = jointListElement1;
body1->mJointsList);
body1->mJointsList = jointListElement1;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(joint->mBody1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) +
"Body " + std::to_string(body1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) +
" added to body");
// Add the joint at the beginning of the linked list of joints of the second body
void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(JointListElement));
JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint,
joint->mBody2->mJointsList);
joint->mBody2->mJointsList = jointListElement2;
body2->mJointsList);
body2->mJointsList = jointListElement2;
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(joint->mBody2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) +
"Body " + std::to_string(body2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) +
" added to body");
}

5
src/engine/DynamicsWorld.h
View File

@ -219,6 +219,11 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Friendship -------------------- //
friend class RigidBody;
friend class Joint;
friend class BallAndSocketJoint;
friend class FixedJoint;
friend class HingeJoint;
friend class SliderJoint;
};
// Get the number of iterations for the velocity constraint solver