Add SolveHingeJointSystem class
This commit is contained in:
Daniel Chappuis
parent
ab02d98f3a
commit
afb34b4355
|
|
@ -134,6 +134,7 @@ SET (REACTPHYSICS3D_HEADERS
|
|||
"src/systems/CollisionDetectionSystem.h"
|
||||
"src/systems/SolveBallAndSocketJointSystem.h"
|
||||
"src/systems/SolveFixedJointSystem.h"
|
||||
"src/systems/SolveHingeJointSystem.h"
|
||||
"src/engine/DynamicsWorld.h"
|
||||
"src/engine/EventListener.h"
|
||||
"src/engine/Island.h"
|
||||
|
|
@ -232,6 +233,7 @@ SET (REACTPHYSICS3D_SOURCES
|
|||
"src/systems/CollisionDetectionSystem.cpp"
|
||||
"src/systems/SolveBallAndSocketJointSystem.cpp"
|
||||
"src/systems/SolveFixedJointSystem.cpp"
|
||||
"src/systems/SolveHingeJointSystem.cpp"
|
||||
"src/engine/DynamicsWorld.cpp"
|
||||
"src/engine/Island.cpp"
|
||||
"src/engine/Material.cpp"
|
||||
|
|
|
|||
|
|
@ -86,7 +86,6 @@ void RigidBody::setType(BodyType type) {
|
|||
// Reset the inverse mass and inverse inertia tensor to zero
|
||||
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0));
|
||||
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
|
||||
mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
|
||||
}
|
||||
else { // If it is a dynamic body
|
||||
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity));
|
||||
|
|
@ -96,9 +95,6 @@ void RigidBody::setType(BodyType type) {
|
|||
}
|
||||
}
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
// Awake the body
|
||||
setIsSleeping(false);
|
||||
|
||||
|
|
@ -130,10 +126,9 @@ const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
|
|||
* @return The 3x3 inverse inertia tensor matrix of the body in world-space
|
||||
* coordinates
|
||||
*/
|
||||
Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
|
||||
const Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
|
||||
|
||||
// Compute and return the inertia tensor in world coordinates
|
||||
return mWorld.mRigidBodyComponents.getInertiaTensorWorldInverse(mEntity);
|
||||
return getInertiaTensorInverseWorld(mWorld, mEntity);
|
||||
}
|
||||
|
||||
// Method that return the mass of the body
|
||||
|
|
@ -192,9 +187,6 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
|
|||
// Compute the inverse local inertia tensor
|
||||
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
|
||||
"Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
|
||||
}
|
||||
|
|
@ -255,9 +247,6 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
|
|||
// Compute the inverse local inertia tensor
|
||||
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
|
||||
"Body " + std::to_string(mEntity.id) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string());
|
||||
}
|
||||
|
|
@ -317,20 +306,6 @@ void RigidBody::setMass(decimal mass) {
|
|||
"Body " + std::to_string(mEntity.id) + ": Set mass=" + std::to_string(mass));
|
||||
}
|
||||
|
||||
// Update the world inverse inertia tensor of the body
|
||||
/// The inertia tensor I_w in world coordinates is computed with the
|
||||
/// local inverse inertia tensor I_b^-1 in body coordinates
|
||||
/// by I_w = R * I_b^-1 * R^T
|
||||
/// where R is the rotation matrix (and R^T its transpose) of the
|
||||
/// current orientation quaternion of the body
|
||||
void RigidBody::updateInertiaTensorInverseWorld() {
|
||||
|
||||
// TODO : Make sure we do this in a system
|
||||
|
||||
Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
|
||||
const Matrix3x3& inverseInertiaLocalTensor = mWorld.mRigidBodyComponents.getInertiaTensorLocalInverse(mEntity);
|
||||
mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
|
||||
}
|
||||
|
||||
// Add a collision shape to the body.
|
||||
/// When you add a collision shape to the body, an internal copy of this
|
||||
|
|
@ -555,9 +530,6 @@ void RigidBody::setTransform(const Transform& transform) {
|
|||
// Update the transform of the body
|
||||
mWorld.mTransformComponents.setTransform(mEntity, transform);
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
// Awake the body if it is sleeping
|
||||
setIsSleeping(false);
|
||||
}
|
||||
|
|
@ -569,7 +541,6 @@ void RigidBody::recomputeMassInformation() {
|
|||
mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(0.0));
|
||||
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0.0));
|
||||
if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
|
||||
if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
|
||||
if (!mIsCenterOfMassSetByUser) mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, Vector3::zero());
|
||||
Matrix3x3 inertiaTensorLocal;
|
||||
inertiaTensorLocal.setToZero();
|
||||
|
|
@ -651,9 +622,6 @@ void RigidBody::recomputeMassInformation() {
|
|||
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
|
||||
}
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
updateInertiaTensorInverseWorld();
|
||||
|
||||
// Update the linear velocity of the center of mass
|
||||
Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity);
|
||||
Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity);
|
||||
|
|
@ -748,6 +716,14 @@ void RigidBody::setIsSleeping(bool isSleeping) {
|
|||
(isSleeping ? "true" : "false"));
|
||||
}
|
||||
|
||||
/// Return the inverse of the inertia tensor in world coordinates.
|
||||
const Matrix3x3 RigidBody::getInertiaTensorInverseWorld(CollisionWorld& world, Entity bodyEntity) {
|
||||
|
||||
Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix();
|
||||
const Matrix3x3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
|
||||
return orientation * inverseInertiaLocalTensor * orientation.getTranspose();
|
||||
}
|
||||
|
||||
// Set whether or not the body is allowed to go to sleep
|
||||
/**
|
||||
* @param isAllowedToSleep True if the body is allowed to sleep
|
||||
|
|
|
|||
|
|
@ -73,12 +73,12 @@ class RigidBody : public CollisionBody {
|
|||
/// Update the transform of the body after a change of the center of mass
|
||||
void updateTransformWithCenterOfMass();
|
||||
|
||||
/// Update the world inverse inertia tensor of the body
|
||||
void updateInertiaTensorInverseWorld();
|
||||
|
||||
/// Set the variable to know whether or not the body is sleeping
|
||||
void setIsSleeping(bool isSleeping);
|
||||
|
||||
/// Return the inverse of the inertia tensor in world coordinates.
|
||||
static const Matrix3x3 getInertiaTensorInverseWorld(CollisionWorld& world, Entity bodyEntity);
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -123,7 +123,7 @@ class RigidBody : public CollisionBody {
|
|||
const Matrix3x3& getInverseInertiaTensorLocal() const;
|
||||
|
||||
/// Return the inverse of the inertia tensor in world coordinates.
|
||||
Matrix3x3 getInertiaTensorInverseWorld() const;
|
||||
const Matrix3x3 getInertiaTensorInverseWorld() const;
|
||||
|
||||
/// Set the local center of mass of the body (in local-space coordinates)
|
||||
void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
|
||||
|
|
@ -215,11 +215,15 @@ class RigidBody : public CollisionBody {
|
|||
|
||||
friend class DynamicsWorld;
|
||||
friend class ContactSolverSystem;
|
||||
friend class DynamicsSystem;
|
||||
friend class Joint;
|
||||
friend class BallAndSocketJoint;
|
||||
friend class SliderJoint;
|
||||
friend class HingeJoint;
|
||||
friend class FixedJoint;
|
||||
friend class SolveBallAndSocketJointSystem;
|
||||
friend class SolveFixedJointSystem;
|
||||
friend class SolveHingeJointSystem;
|
||||
};
|
||||
|
||||
// Return a reference to the material properties of the rigid body
|
||||
|
|
|
|||
|
|
@ -411,6 +411,7 @@ class HingeJointComponents : public Components {
|
|||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class BroadPhaseSystem;
|
||||
friend class SolveHingeJointSystem;
|
||||
};
|
||||
|
||||
// Return a pointer to a given joint
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
|
|||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) +
|
||||
sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) +
|
||||
sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) {
|
||||
|
|
@ -77,8 +77,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
|
||||
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
|
||||
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
|
||||
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
|
||||
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
|
||||
Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
|
||||
|
|
@ -109,7 +108,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
|
||||
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
|
||||
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
|
||||
|
|
@ -143,7 +141,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
mInitMasses = newInitMasses;
|
||||
mInverseMasses = newInverseMasses;
|
||||
mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
|
||||
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
|
||||
mConstrainedLinearVelocities = newConstrainedLinearVelocities;
|
||||
mConstrainedAngularVelocities = newConstrainedAngularVelocities;
|
||||
mSplitLinearVelocities = newSplitLinearVelocities;
|
||||
|
|
@ -179,7 +176,6 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
|
|||
mInitMasses[index] = decimal(1.0);
|
||||
mInverseMasses[index] = decimal(1.0);
|
||||
new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
|
||||
new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
|
||||
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
|
||||
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
|
||||
new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
|
||||
|
|
@ -223,7 +219,6 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
|
|||
mInitMasses[destIndex] = mInitMasses[srcIndex];
|
||||
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
|
||||
new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]);
|
||||
new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
|
||||
new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
|
||||
new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
|
||||
new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
|
||||
|
|
@ -266,7 +261,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
decimal initMass1 = mInitMasses[index1];
|
||||
decimal inverseMass1 = mInverseMasses[index1];
|
||||
Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
|
||||
Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
|
||||
Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
|
||||
Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
|
||||
Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
|
||||
|
|
@ -300,7 +294,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
mInitMasses[index2] = initMass1;
|
||||
mInverseMasses[index2] = inverseMass1;
|
||||
mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
|
||||
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
|
||||
new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
|
||||
new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
|
||||
new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
|
||||
|
|
@ -337,7 +330,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
|
|||
mExternalForces[index].~Vector3();
|
||||
mExternalTorques[index].~Vector3();
|
||||
mInverseInertiaTensorsLocal[index].~Matrix3x3();
|
||||
mInverseInertiaTensorsWorld[index].~Matrix3x3();
|
||||
mConstrainedLinearVelocities[index].~Vector3();
|
||||
mConstrainedAngularVelocities[index].~Vector3();
|
||||
mSplitLinearVelocities[index].~Vector3();
|
||||
|
|
|
|||
|
|
@ -109,9 +109,6 @@ class RigidBodyComponents : public Components {
|
|||
/// Array with the inverse of the inertia tensor of each component
|
||||
Matrix3x3* mInverseInertiaTensorsLocal;
|
||||
|
||||
/// Array with the inverse of the world inertia tensor of each component
|
||||
Matrix3x3* mInverseInertiaTensorsWorld;
|
||||
|
||||
/// Array with the constrained linear velocity of each component
|
||||
Vector3* mConstrainedLinearVelocities;
|
||||
|
||||
|
|
@ -246,9 +243,6 @@ class RigidBodyComponents : public Components {
|
|||
/// Return the inverse local inertia tensor of an entity
|
||||
const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
|
||||
|
||||
/// Return the inverse world inertia tensor of an entity
|
||||
const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
|
||||
|
||||
/// Set the external force of an entity
|
||||
void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
|
||||
|
||||
|
|
@ -270,9 +264,6 @@ class RigidBodyComponents : public Components {
|
|||
/// Set the inverse local inertia tensor of an entity
|
||||
void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
|
||||
|
||||
/// Set the inverse world inertia tensor of an entity
|
||||
void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
|
||||
|
||||
/// Return the constrained linear velocity of an entity
|
||||
const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
|
||||
|
||||
|
|
@ -286,10 +277,10 @@ class RigidBodyComponents : public Components {
|
|||
const Vector3& getSplitAngularVelocity(Entity bodyEntity) const;
|
||||
|
||||
/// Return the constrained position of an entity
|
||||
const Vector3& getConstrainedPosition(Entity bodyEntity);
|
||||
Vector3& getConstrainedPosition(Entity bodyEntity);
|
||||
|
||||
/// Return the constrained orientation of an entity
|
||||
const Quaternion& getConstrainedOrientation(Entity bodyEntity);
|
||||
Quaternion& getConstrainedOrientation(Entity bodyEntity);
|
||||
|
||||
/// Return the local center of mass of an entity
|
||||
const Vector3& getCenterOfMassLocal(Entity bodyEntity);
|
||||
|
|
@ -348,6 +339,7 @@ class RigidBodyComponents : public Components {
|
|||
friend class ContactSolverSystem;
|
||||
friend class SolveBallAndSocketJointSystem;
|
||||
friend class SolveFixedJointSystem;
|
||||
friend class SolveHingeJointSystem;
|
||||
friend class DynamicsSystem;
|
||||
friend class BallAndSocketJoint;
|
||||
friend class FixedJoint;
|
||||
|
|
@ -515,14 +507,6 @@ inline const Matrix3x3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity
|
|||
return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
|
||||
}
|
||||
|
||||
// Return the inverse world inertia tensor of an entity
|
||||
inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
|
||||
}
|
||||
|
||||
// Set the external force of an entity
|
||||
inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
|
||||
|
||||
|
|
@ -579,14 +563,6 @@ inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity,
|
|||
mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
|
||||
}
|
||||
|
||||
// Set the inverse world inertia tensor of an entity
|
||||
inline void RigidBodyComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
|
||||
}
|
||||
|
||||
// Return the constrained linear velocity of an entity
|
||||
inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const {
|
||||
|
||||
|
|
@ -620,7 +596,7 @@ inline const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEn
|
|||
}
|
||||
|
||||
// Return the constrained position of an entity
|
||||
inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) {
|
||||
inline Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
|
|
@ -628,7 +604,7 @@ inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEnt
|
|||
}
|
||||
|
||||
// Return the constrained orientation of an entity
|
||||
inline const Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) {
|
||||
inline Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
|
|
|
|||
|
|
@ -31,9 +31,6 @@
|
|||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
const decimal HingeJoint::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world) {
|
||||
|
||||
|
|
@ -67,651 +64,21 @@ HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo
|
|||
// 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(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
|
||||
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mWorld.mHingeJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mHingeJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
|
||||
bool oldIsLowerLimitViolated = mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity);
|
||||
bool isLowerLimitViolated = lowerLimitError <= 0;
|
||||
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated);
|
||||
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
bool oldIsUpperLimitViolated = mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity);
|
||||
bool isUpperLimitViolated = upperLimitError <= 0;
|
||||
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated);
|
||||
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
Vector3 a1 = orientationBody1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
|
||||
Vector3 a2 = orientationBody2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
|
||||
a1.normalize();
|
||||
a2.normalize();
|
||||
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2.cross(a1));
|
||||
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2.cross(a1));
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR2World(mEntity));
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
|
||||
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
|
||||
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
|
||||
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mWorld.mHingeJointsComponents.getI1(mEntity) * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mWorld.mHingeJointsComponents.getI2(mEntity) * skewSymmetricMatrixU2.getTranspose();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the translation constraints
|
||||
Vector3& bTranslation = mWorld.mHingeJointsComponents.getBiasTranslation(mEntity);
|
||||
bTranslation.setToZero();
|
||||
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
bTranslation = biasFactor * (x2 + mWorld.mHingeJointsComponents.getR2World(mEntity) - x1 - mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
mWorld.mHingeJointsComponents.setBiasTranslation(mEntity, bTranslation);
|
||||
}
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 i1B2CrossA1 = i1 * b2CrossA1;
|
||||
Vector3 i1C2CrossA1 = i1 * c2CrossA1;
|
||||
Vector3 i2B2CrossA1 = i2 * b2CrossA1;
|
||||
Vector3 i2C2CrossA1 = i2 * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(i1B2CrossA1) +
|
||||
b2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(i1C2CrossA1) +
|
||||
b2CrossA1.dot(i2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(i1B2CrossA1) +
|
||||
c2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) +
|
||||
c2CrossA1.dot(i2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the rotation constraints
|
||||
Vector2& biasRotation = mWorld.mHingeJointsComponents.getBiasRotation(mEntity);
|
||||
biasRotation.setToZero();
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mWorld.mHingeJointsComponents.setBiasRotation(mEntity, biasFactor * Vector2(a1.dot(b2), a1.dot(c2)));
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!constraintSolverData.isWarmStartingActive) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
|
||||
Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
|
||||
impulseTranslation.setToZero();
|
||||
impulseRotation.setToZero();
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
|
||||
}
|
||||
|
||||
// If the motor or limits are enabled
|
||||
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) ||
|
||||
(mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) ||
|
||||
mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)))) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
||||
decimal inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
|
||||
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError);
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// 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];
|
||||
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
|
||||
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
|
||||
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
|
||||
const Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
|
||||
|
||||
const decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
const decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
|
||||
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
|
||||
Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mWorld.mHingeJointsComponents.getC2CrossA1(mEntity) * impulseRotation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
|
||||
const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint
|
||||
const Vector3 motorImpulse = -mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
|
||||
Vector3 linearImpulseBody1 = -impulseTranslation;
|
||||
Vector3 angularImpulseBody1 = impulseTranslation.cross(mWorld.mHingeJointsComponents.getR1World(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 += rotationImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
angularImpulseBody1 += limitsImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
angularImpulseBody1 += motorImpulse;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mWorld.mHingeJointsComponents.getI1(mEntity) * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = -impulseTranslation.cross(mWorld.mHingeJointsComponents.getR2World(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 += -rotationImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
|
||||
angularImpulseBody2 += -limitsImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
|
||||
angularImpulseBody2 += -motorImpulse;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * impulseTranslation;
|
||||
w2 += mWorld.mHingeJointsComponents.getI2(mEntity) * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// 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];
|
||||
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
|
||||
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
|
||||
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
|
||||
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
|
||||
|
||||
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
const decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambdaTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity) *
|
||||
(-JvTranslation - mWorld.mHingeJointsComponents.getBiasTranslation(mEntity));
|
||||
mWorld.mHingeJointsComponents.setImpulseTranslation(mEntity, deltaLambdaTranslation + mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World);
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * deltaLambdaTranslation;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 rotation constraints
|
||||
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
|
||||
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
|
||||
Vector2 deltaLambdaRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity) *
|
||||
(-JvRotation - mWorld.mHingeJointsComponents.getBiasRotation(mEntity));
|
||||
mWorld.mHingeJointsComponents.setImpulseRotation(mEntity, deltaLambdaRotation + mWorld.mHingeJointsComponents.getImpulseRotation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x -
|
||||
c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
|
||||
// Compute J*v for the lower limit constraint
|
||||
const decimal JvLowerLimit = (w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mWorld.mHingeJointsComponents.getBLowerLimit(mEntity));
|
||||
decimal lambdaTemp = impulseLowerLimit;
|
||||
impulseLowerLimit = std::max(impulseLowerLimit + deltaLambdaLower, decimal(0.0));
|
||||
deltaLambdaLower = impulseLowerLimit - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, impulseLowerLimit);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
|
||||
|
||||
// Compute J*v for the upper limit constraint
|
||||
const decimal JvUpperLimit = -(w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mWorld.mHingeJointsComponents.getBUpperLimit(mEntity));
|
||||
decimal lambdaTemp = impulseUpperLimit;
|
||||
impulseUpperLimit = std::max(impulseUpperLimit + deltaLambdaUpper, decimal(0.0));
|
||||
deltaLambdaUpper = impulseUpperLimit - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, impulseUpperLimit);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Motor --------------- //
|
||||
|
||||
// If the motor is enabled
|
||||
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity)) {
|
||||
|
||||
decimal impulseMotor = mWorld.mHingeJointsComponents.getImpulseMotor(mEntity);
|
||||
|
||||
// Compute J*v for the motor
|
||||
const decimal JvMotor = a1.dot(w1 - w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the motor
|
||||
const decimal maxMotorImpulse = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity) * constraintSolverData.timeStep;
|
||||
decimal deltaLambdaMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity) * (-JvMotor - mWorld.mHingeJointsComponents.getMotorSpeed(mEntity));
|
||||
decimal lambdaTemp = impulseMotor;
|
||||
impulseMotor = clamp(impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
deltaLambdaMotor = impulseMotor - lambdaTemp;
|
||||
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, impulseMotor);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do
|
||||
// do not execute this method
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != 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));
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
|
||||
|
||||
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
|
||||
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
|
||||
|
||||
Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
|
||||
Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
|
||||
|
||||
Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
|
||||
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
|
||||
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(body1Entity);
|
||||
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mWorld.mHingeJointsComponents.setR1World(mEntity, q1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mHingeJointsComponents.setR2World(mEntity, q2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
|
||||
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0);
|
||||
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0);
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
a1 = q1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
|
||||
Vector3 a2 = q2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
|
||||
a1.normalize();
|
||||
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
|
||||
a2.normalize();
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
b2CrossA1 = b2.cross(a1);
|
||||
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2CrossA1);
|
||||
c2CrossA1 = c2.cross(a1);
|
||||
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2CrossA1);
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
|
||||
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,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
inverseMassMatrixTranslation = massMatrix.getInverse();
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, inverseMassMatrixTranslation);
|
||||
}
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 lambdaTranslation = inverseMassMatrixTranslation * (-errorTranslation);
|
||||
|
||||
// Compute the impulse of body 1
|
||||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
Vector3 w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
|
||||
Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = i1 * b2CrossA1;
|
||||
Vector3 I1C2CrossA1 = i1 * c2CrossA1;
|
||||
Vector3 I2B2CrossA1 = i2 * b2CrossA1;
|
||||
Vector3 I2C2CrossA1 = i2 * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
|
||||
b2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
|
||||
b2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
|
||||
c2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
|
||||
c2CrossA1.dot(I2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
|
||||
inverseMassMatrixRotation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the position error for the 3 rotation constraints
|
||||
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector2 lambdaRotation = inverseMassMatrixRotation * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
|
||||
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
|
||||
}
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = i1 * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
|
||||
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
|
||||
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
|
||||
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
|
||||
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -824,88 +191,6 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
|
|||
}
|
||||
}
|
||||
|
||||
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
|
||||
decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
|
||||
|
||||
// Convert it into the range [-2*pi; 2*pi]
|
||||
angle = std::fmod(angle, PI_TIMES_2);
|
||||
|
||||
// Convert it into the range [-pi; pi]
|
||||
if (angle < -PI) {
|
||||
return angle + PI_TIMES_2;
|
||||
}
|
||||
else if (angle > PI) {
|
||||
return angle - PI_TIMES_2;
|
||||
}
|
||||
else {
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
|
||||
// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
|
||||
// two angle limits in arguments.
|
||||
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const {
|
||||
if (upperLimitAngle <= lowerLimitAngle) {
|
||||
return inputAngle;
|
||||
}
|
||||
else if (inputAngle > upperLimitAngle) {
|
||||
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 = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
|
||||
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
|
||||
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
|
||||
}
|
||||
else {
|
||||
return inputAngle;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, const Quaternion& orientationBody2) {
|
||||
|
||||
decimal hingeAngle;
|
||||
|
||||
// Compute the current orientation difference between the two bodies
|
||||
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
|
||||
currentOrientationDiff.normalize();
|
||||
|
||||
// Compute the relative rotation considering the initial orientation difference
|
||||
Quaternion relativeRotation = currentOrientationDiff * mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity);
|
||||
relativeRotation.normalize();
|
||||
|
||||
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
|
||||
// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
|
||||
// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
|
||||
// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
|
||||
// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
|
||||
// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
|
||||
// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
|
||||
decimal cosHalfAngle = relativeRotation.w;
|
||||
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
|
||||
|
||||
// Compute the dot product of the relative rotation axis and the hinge axis
|
||||
decimal dotProduct = relativeRotation.getVectorV().dot(mWorld.mHingeJointsComponents.getA1(mEntity));
|
||||
|
||||
// If the relative rotation axis and the hinge axis are pointing the same direction
|
||||
if (dotProduct >= decimal(0.0)) {
|
||||
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
|
||||
}
|
||||
else {
|
||||
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
|
||||
}
|
||||
|
||||
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
|
||||
hingeAngle = computeNormalizedAngle(hingeAngle);
|
||||
|
||||
// Compute and return the corresponding angle near one the two limits
|
||||
return computeCorrespondingAngleNearLimits(hingeAngle,
|
||||
mWorld.mHingeJointsComponents.getLowerLimit(mEntity),
|
||||
mWorld.mHingeJointsComponents.getUpperLimit(mEntity));
|
||||
}
|
||||
|
||||
// Return true if the limits of the joint are enabled
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -144,9 +144,6 @@ class HingeJoint : public Joint {
|
|||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
// Beta value for the bias factor of position correction
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
|
||||
|
|
@ -155,20 +152,6 @@ class HingeJoint : public Joint {
|
|||
/// Reset the limits
|
||||
void resetLimits();
|
||||
|
||||
/// Given an angle in radian, this method returns the corresponding
|
||||
/// angle in the range [-pi; pi]
|
||||
decimal computeNormalizedAngle(decimal angle) const;
|
||||
|
||||
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
|
||||
/// two angle limits in arguments.
|
||||
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
|
||||
decimal upperLimitAngle) const;
|
||||
|
||||
/// Compute the current angle around the hinge axis
|
||||
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1,
|
||||
const Quaternion& orientationBody2);
|
||||
|
||||
/// Return the number of bytes used by the joint
|
||||
virtual size_t getSizeInBytes() const override;
|
||||
|
||||
|
|
|
|||
|
|
@ -88,8 +88,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
|
|
@ -529,8 +529,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
|
|||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
// Recompute the inertia tensor of bodies
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
|
|
|
|||
|
|
@ -50,11 +50,11 @@ using namespace std;
|
|||
DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
|
||||
: CollisionWorld(worldSettings, logger, profiler),
|
||||
mIslands(mMemoryManager.getSingleFrameAllocator()),
|
||||
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
|
||||
mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
|
||||
mProxyShapesComponents, mConfig),
|
||||
mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
||||
mBallAndSocketJointsComponents, mFixedJointsComponents),
|
||||
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
||||
mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
||||
mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents),
|
||||
mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
||||
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
|
||||
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
|
||||
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
|
||||
|
|
@ -249,9 +249,6 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
|
|||
// Compute the inverse mass
|
||||
mRigidBodyComponents.setMassInverse(entity, decimal(1.0) / mRigidBodyComponents.getInitMass(entity));
|
||||
|
||||
// Update the world inverse inertia tensor
|
||||
rigidBody->updateInertiaTensorInverseWorld();
|
||||
|
||||
// Add the rigid body to the physics world
|
||||
mBodies.add(rigidBody);
|
||||
mRigidBodies.add(rigidBody);
|
||||
|
|
|
|||
|
|
@ -33,17 +33,19 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||
ConstraintSolverSystem::ConstraintSolverSystem(DynamicsWorld& world, Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||
FixedJointComponents& fixedJointComponents)
|
||||
FixedJointComponents& fixedJointComponents,
|
||||
HingeJointComponents& hingeJointComponents)
|
||||
: mIsWarmStartingActive(true), mIslands(islands),
|
||||
mConstraintSolverData(rigidBodyComponents, jointComponents),
|
||||
mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
|
||||
mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
|
||||
mSolveBallAndSocketJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
|
||||
mSolveFixedJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
|
||||
mSolveHingeJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, hingeJointComponents),
|
||||
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
|
||||
mFixedJointComponents(fixedJointComponents) {
|
||||
mFixedJointComponents(fixedJointComponents), mHingeJointComponents(hingeJointComponents) {
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
|
|
@ -69,13 +71,17 @@ void ConstraintSolverSystem::initialize(decimal dt) {
|
|||
mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
mSolveFixedJointSystem.setTimeStep(dt);
|
||||
mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
mSolveHingeJointSystem.setTimeStep(dt);
|
||||
mSolveHingeJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
|
||||
mSolveBallAndSocketJointSystem.initBeforeSolve();
|
||||
mSolveFixedJointSystem.initBeforeSolve();
|
||||
mSolveHingeJointSystem.initBeforeSolve();
|
||||
|
||||
if (mIsWarmStartingActive) {
|
||||
mSolveBallAndSocketJointSystem.warmstart();
|
||||
mSolveFixedJointSystem.warmstart();
|
||||
mSolveHingeJointSystem.warmstart();
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -84,7 +90,8 @@ void ConstraintSolverSystem::initialize(decimal dt) {
|
|||
// TODO : DELETE THIS
|
||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -110,6 +117,7 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
|
|||
|
||||
mSolveBallAndSocketJointSystem.solveVelocityConstraint();
|
||||
mSolveFixedJointSystem.solveVelocityConstraint();
|
||||
mSolveHingeJointSystem.solveVelocityConstraint();
|
||||
|
||||
// For each joint
|
||||
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
||||
|
|
@ -117,7 +125,8 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
|
|||
// TODO : DELETE THIS
|
||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -133,6 +142,7 @@ void ConstraintSolverSystem::solvePositionConstraints() {
|
|||
|
||||
mSolveBallAndSocketJointSystem.solvePositionConstraint();
|
||||
mSolveFixedJointSystem.solvePositionConstraint();
|
||||
mSolveHingeJointSystem.solvePositionConstraint();
|
||||
|
||||
// For each joint
|
||||
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
||||
|
|
@ -140,7 +150,8 @@ void ConstraintSolverSystem::solvePositionConstraints() {
|
|||
// TODO : DELETE THIS
|
||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
#include "engine/Islands.h"
|
||||
#include "systems/SolveBallAndSocketJointSystem.h"
|
||||
#include "systems/SolveFixedJointSystem.h"
|
||||
#include "systems/SolveHingeJointSystem.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
|
|
@ -165,6 +166,9 @@ class ConstraintSolverSystem {
|
|||
/// Solver for the FixedJoint constraints
|
||||
SolveFixedJointSystem mSolveFixedJointSystem;
|
||||
|
||||
/// Solver for the HingeJoint constraints
|
||||
SolveHingeJointSystem mSolveHingeJointSystem;
|
||||
|
||||
// TODO : Delete this
|
||||
JointComponents& mJointComponents;
|
||||
|
||||
|
|
@ -174,6 +178,9 @@ class ConstraintSolverSystem {
|
|||
// TODO : Delete this
|
||||
FixedJointComponents& mFixedJointComponents;
|
||||
|
||||
// TODO : Delete this
|
||||
HingeJointComponents& mHingeJointComponents;
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
/// Pointer to the profiler
|
||||
|
|
@ -185,11 +192,11 @@ class ConstraintSolverSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||
ConstraintSolverSystem(DynamicsWorld& world, Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||
FixedJointComponents& fixedJointComponents);
|
||||
FixedJointComponents& fixedJointComponents, HingeJointComponents &hingeJointComponents);
|
||||
|
||||
/// Destructor
|
||||
~ConstraintSolverSystem() = default;
|
||||
|
|
|
|||
|
|
@ -43,10 +43,10 @@ const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2);
|
|||
const decimal ContactSolverSystem::SLOP = decimal(0.01);
|
||||
|
||||
// Constructor
|
||||
ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
|
||||
ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, DynamicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents,
|
||||
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
|
||||
const WorldSettings& worldSettings)
|
||||
:mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr),
|
||||
:mMemoryManager(memoryManager), mWorld(world), mContactConstraints(nullptr), mContactPoints(nullptr),
|
||||
mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr),
|
||||
mBodyComponents(bodyComponents), mRigidBodyComponents(rigidBodyComponents),
|
||||
mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true),
|
||||
|
|
@ -140,8 +140,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
|
|||
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
|
||||
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity());
|
||||
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity());
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1);
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2);
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity());
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity());
|
||||
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
|
||||
|
|
|
|||
|
|
@ -284,6 +284,9 @@ class ContactSolverSystem {
|
|||
/// Memory manager
|
||||
MemoryManager& mMemoryManager;
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Current time step
|
||||
decimal mTimeStep;
|
||||
|
||||
|
|
@ -360,7 +363,7 @@ class ContactSolverSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
|
||||
ContactSolverSystem(MemoryManager& memoryManager, DynamicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents,
|
||||
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
|
||||
const WorldSettings& worldSettings);
|
||||
|
||||
|
|
|
|||
|
|
@ -25,12 +25,14 @@
|
|||
|
||||
// Libraries
|
||||
#include "systems/DynamicsSystem.h"
|
||||
#include "body/RigidBody.h"
|
||||
#include "engine/DynamicsWorld.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
|
||||
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
|
||||
DynamicsSystem::DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
|
||||
mGravity(gravity) {
|
||||
|
||||
}
|
||||
|
|
@ -95,14 +97,6 @@ void DynamicsSystem::updateBodiesState() {
|
|||
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
|
||||
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
|
||||
}
|
||||
|
||||
// Update the world inverse inertia tensor of the body
|
||||
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
|
||||
const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
|
||||
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate the velocities of rigid bodies.
|
||||
|
|
@ -130,7 +124,7 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
|
|||
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
|
||||
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
|
||||
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
|
||||
RigidBody::getInertiaTensorInverseWorld(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
|
||||
}
|
||||
|
||||
// Apply gravity force
|
||||
|
|
|
|||
|
|
@ -33,6 +33,8 @@
|
|||
|
||||
namespace reactphysics3d {
|
||||
|
||||
class DynamicsWorld;
|
||||
|
||||
// Class DynamicsSystem
|
||||
/**
|
||||
* This class is responsible to compute and update the dynamics of the bodies that are simulated
|
||||
|
|
@ -44,6 +46,9 @@ class DynamicsSystem {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
|
|
@ -67,7 +72,7 @@ class DynamicsSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
bool& isGravityEnabled, Vector3& gravity);
|
||||
|
||||
/// Destructor
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
// Libraries
|
||||
#include "systems/SolveBallAndSocketJointSystem.h"
|
||||
#include "engine/DynamicsWorld.h"
|
||||
#include "body/RigidBody.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
|
@ -33,11 +34,11 @@ using namespace reactphysics3d;
|
|||
const decimal SolveBallAndSocketJointSystem::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents,
|
||||
SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents)
|
||||
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
|
||||
mTimeStep(0), mIsWarmStartingActive(true) {
|
||||
|
||||
|
|
@ -55,13 +56,9 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// TODO : Remove this and use compoents instead of pointers to bodies
|
||||
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
|
||||
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
|
||||
mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
|
||||
mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -260,13 +257,9 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// TODO : Remove this and use compoents instead of pointers to bodies
|
||||
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
|
||||
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
|
||||
mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
|
||||
mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
|
|
|
|||
|
|
@ -35,6 +35,9 @@
|
|||
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Forward declarations
|
||||
class DynamicsWorld;
|
||||
|
||||
// Class SolveBallAndSocketJointSystem
|
||||
/**
|
||||
* This class is responsible to solve the BallAndSocketJoint constraints
|
||||
|
|
@ -50,6 +53,9 @@ class SolveBallAndSocketJointSystem {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
|
|
@ -79,7 +85,7 @@ class SolveBallAndSocketJointSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents,
|
||||
SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents);
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
// Libraries
|
||||
#include "systems/SolveFixedJointSystem.h"
|
||||
#include "engine/DynamicsWorld.h"
|
||||
#include "body/RigidBody.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
|
@ -33,11 +34,11 @@ using namespace reactphysics3d;
|
|||
const decimal SolveFixedJointSystem::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
SolveFixedJointSystem::SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents,
|
||||
SolveFixedJointSystem::SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
FixedJointComponents& fixedJointComponents)
|
||||
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
mJointComponents(jointComponents), mFixedJointComponents(fixedJointComponents),
|
||||
mTimeStep(0), mIsWarmStartingActive(true) {
|
||||
|
||||
|
|
@ -55,13 +56,9 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// TODO : Remove this and use compoents instead of pointers to bodies
|
||||
RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
|
||||
RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
|
||||
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
|
||||
mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -91,8 +88,8 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]);
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
|
@ -346,13 +343,9 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// TODO : Remove this and use compoents instead of pointers to bodies
|
||||
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
|
||||
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
|
||||
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
|
||||
mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -439,10 +432,10 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
Vector3 x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3 x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
Quaternion q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
|
||||
|
|
@ -536,10 +529,5 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
mRigidBodyComponents.mConstrainedPositions[componentIndexBody1] = x1;
|
||||
mRigidBodyComponents.mConstrainedPositions[componentIndexBody2] = x2;
|
||||
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] = q1;
|
||||
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] = q2;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@
|
|||
|
||||
namespace reactphysics3d {
|
||||
|
||||
class DynamicsWorld;
|
||||
|
||||
// Class SolveFixedJointSystem
|
||||
/**
|
||||
* This class is responsible to solve the FixedJoint constraints
|
||||
|
|
@ -50,6 +52,9 @@ class SolveFixedJointSystem {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
|
|
@ -79,7 +84,7 @@ class SolveFixedJointSystem {
|
|||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents, FixedJointComponents& fixedJointComponents);
|
||||
|
||||
/// Destructor
|
||||
|
|
|
|||
902
src/systems/SolveHingeJointSystem.cpp
Normal file
902
src/systems/SolveHingeJointSystem.cpp
Normal file
|
|
@ -0,0 +1,902 @@
|
|||
/********************************************************************************
|
||||
* 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 "systems/SolveHingeJointSystem.h"
|
||||
#include "engine/DynamicsWorld.h"
|
||||
#include "body/RigidBody.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
const decimal SolveHingeJointSystem::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
SolveHingeJointSystem::SolveHingeJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
HingeJointComponents& hingeJointComponents)
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
mJointComponents(jointComponents), mHingeJointComponents(hingeJointComponents),
|
||||
mTimeStep(0), mIsWarmStartingActive(true) {
|
||||
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
void SolveHingeJointSystem::initBeforeSolve() {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mHingeJointComponents.mR1World[i] = orientationBody1 * mHingeJointComponents.mLocalAnchorPointBody1[i];
|
||||
mHingeJointComponents.mR2World[i] = orientationBody2 * mHingeJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
a1 = orientationBody1 * mHingeJointComponents.mHingeLocalAxisBody1[i];
|
||||
Vector3 a2 = orientationBody2 * mHingeJointComponents.mHingeLocalAxisBody2[i];
|
||||
|
||||
a1.normalize();
|
||||
a2.normalize();
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
mHingeJointComponents.mB2CrossA1[i] = b2.cross(a1);
|
||||
mHingeJointComponents.mC2CrossA1[i] = c2.cross(a1);
|
||||
|
||||
// Compute the bias "b" of the rotation constraints
|
||||
mHingeJointComponents.mBiasRotation[i].setToZero();
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBiasRotation[i] = biasFactor * Vector2(a1.dot(b2), a1.dot(c2));
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
|
||||
decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
|
||||
Matrix3x3& inverseMassMatrixTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i];
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
|
||||
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity);
|
||||
|
||||
// Compute the bias "b" of the translation constraints
|
||||
mHingeJointComponents.mBiasTranslation[i].setToZero();
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBiasTranslation[i] = biasFactor * (x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
|
||||
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 i1B2CrossA1 = i1 * b2CrossA1;
|
||||
Vector3 i1C2CrossA1 = i1 * c2CrossA1;
|
||||
Vector3 i2B2CrossA1 = i2 * b2CrossA1;
|
||||
Vector3 i2C2CrossA1 = i2 * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(i1B2CrossA1) + b2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(i1C2CrossA1) + b2CrossA1.dot(i2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(i1B2CrossA1) + c2CrossA1.dot(i2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
|
||||
if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
mHingeJointComponents.mImpulseTranslation[i].setToZero();
|
||||
mHingeJointComponents.mImpulseRotation[i].setToZero();
|
||||
mHingeJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
|
||||
mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
|
||||
mHingeJointComponents.mImpulseMotor[i] = decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal hingeAngle = computeCurrentHingeAngle(jointEntity, orientationBody1, orientationBody2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mHingeJointComponents.mLowerLimit[i];
|
||||
decimal upperLimitError = mHingeJointComponents.mUpperLimit[i] - hingeAngle;
|
||||
bool oldIsLowerLimitViolated = mHingeJointComponents.mIsLowerLimitViolated[i];
|
||||
bool isLowerLimitViolated = lowerLimitError <= 0;
|
||||
mHingeJointComponents.mIsLowerLimitViolated[i] = isLowerLimitViolated;
|
||||
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||
mHingeJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
|
||||
}
|
||||
bool oldIsUpperLimitViolated = mHingeJointComponents.mIsUpperLimitViolated[i];
|
||||
bool isUpperLimitViolated = upperLimitError <= 0;
|
||||
mHingeJointComponents.mIsUpperLimitViolated[i] = isUpperLimitViolated;
|
||||
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||
mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
|
||||
}
|
||||
|
||||
// If the motor or limits are enabled
|
||||
if (mHingeJointComponents.mIsMotorEnabled[i] ||
|
||||
(mHingeJointComponents.mIsLimitEnabled[i] && (mHingeJointComponents.mIsLowerLimitViolated[i] ||
|
||||
mHingeJointComponents.mIsUpperLimitViolated[i]))) {
|
||||
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
||||
decimal inverseMassMatrixLimitMotor = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = inverseMassMatrixLimitMotor;
|
||||
|
||||
if (mHingeJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mHingeJointComponents.mBLowerLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mHingeJointComponents.mBUpperLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void SolveHingeJointSystem::warmstart() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Get the velocities
|
||||
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
|
||||
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& impulseTranslation = mHingeJointComponents.mImpulseTranslation[i];
|
||||
const Vector2& impulseRotation = mHingeJointComponents.mImpulseRotation[i];
|
||||
|
||||
const decimal impulseLowerLimit = mHingeJointComponents.mImpulseLowerLimit[i];
|
||||
const decimal impulseUpperLimit = mHingeJointComponents.mImpulseUpperLimit[i];
|
||||
|
||||
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
const Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
|
||||
Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mHingeJointComponents.mC2CrossA1[i] * impulseRotation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
|
||||
const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint
|
||||
const Vector3 motorImpulse = -mHingeJointComponents.mImpulseMotor[i] * a1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
|
||||
Vector3 linearImpulseBody1 = -impulseTranslation;
|
||||
Vector3 angularImpulseBody1 = impulseTranslation.cross(mHingeJointComponents.mR1World[i]);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 += rotationImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
angularImpulseBody1 += limitsImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
angularImpulseBody1 += motorImpulse;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mHingeJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = -impulseTranslation.cross(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 += -rotationImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
|
||||
angularImpulseBody2 += -limitsImpulse;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
|
||||
angularImpulseBody2 += -motorImpulse;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * impulseTranslation;
|
||||
w2 += mHingeJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void SolveHingeJointSystem::solveVelocityConstraint() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Get the velocities
|
||||
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
|
||||
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
|
||||
|
||||
const Vector3& r1World = mHingeJointComponents.mR1World[i];
|
||||
const Vector3& r2World = mHingeJointComponents.mR2World[i];
|
||||
|
||||
const Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
||||
const decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] *
|
||||
(-JvTranslation - mHingeJointComponents.mBiasTranslation[i]);
|
||||
mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World);
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * deltaLambdaTranslation;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
||||
// Compute J*v for the 2 rotation constraints
|
||||
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
|
||||
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
|
||||
Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mHingeJointComponents.mBiasRotation[i]);
|
||||
mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
|
||||
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
|
||||
angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mHingeJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mHingeJointComponents.mIsLowerLimitViolated[i]) {
|
||||
|
||||
// Compute J*v for the lower limit constraint
|
||||
const decimal JvLowerLimit = (w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mHingeJointComponents.mBLowerLimit[i]);
|
||||
decimal lambdaTemp = mHingeJointComponents.mImpulseLowerLimit[i];
|
||||
mHingeJointComponents.mImpulseLowerLimit[i] = std::max(mHingeJointComponents.mImpulseLowerLimit[i] + deltaLambdaLower, decimal(0.0));
|
||||
deltaLambdaLower = mHingeJointComponents.mImpulseLowerLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mHingeJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
// Compute J*v for the upper limit constraint
|
||||
const decimal JvUpperLimit = -(w2 - w1).dot(a1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mHingeJointComponents.mBUpperLimit[i]);
|
||||
decimal lambdaTemp = mHingeJointComponents.mImpulseUpperLimit[i];
|
||||
mHingeJointComponents.mImpulseUpperLimit[i] = std::max(mHingeJointComponents.mImpulseUpperLimit[i] + deltaLambdaUpper, decimal(0.0));
|
||||
deltaLambdaUpper = mHingeJointComponents.mImpulseUpperLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Motor --------------- //
|
||||
|
||||
// If the motor is enabled
|
||||
if (mHingeJointComponents.mIsMotorEnabled[i]) {
|
||||
|
||||
// Compute J*v for the motor
|
||||
const decimal JvMotor = a1.dot(w1 - w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the motor
|
||||
const decimal maxMotorImpulse = mHingeJointComponents.mMaxMotorTorque[i] * mTimeStep;
|
||||
decimal deltaLambdaMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i] * (-JvMotor - mHingeJointComponents.mMotorSpeed[i]);
|
||||
decimal lambdaTemp = mHingeJointComponents.mImpulseMotor[i];
|
||||
mHingeJointComponents.mImpulseMotor[i] = clamp(mHingeJointComponents.mImpulseMotor[i] + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
deltaLambdaMotor = mHingeJointComponents.mImpulseMotor[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void SolveHingeJointSystem::solvePositionConstraint() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mHingeJointComponents.mR1World[i] = q1 * mHingeJointComponents.mLocalAnchorPointBody1[i];
|
||||
mHingeJointComponents.mR2World[i] = q2 * mHingeJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
|
||||
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
|
||||
const decimal body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
|
||||
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
|
||||
0, inverseMassBodies, 0,
|
||||
0, 0, inverseMassBodies) +
|
||||
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
|
||||
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
|
||||
Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
|
||||
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
||||
// Compute vectors needed in the Jacobian
|
||||
a1 = q1 * mHingeJointComponents.mHingeLocalAxisBody1[i];
|
||||
Vector3 a2 = q2 * mHingeJointComponents.mHingeLocalAxisBody2[i];
|
||||
a1.normalize();
|
||||
mHingeJointComponents.mA1[i] = a1;
|
||||
a2.normalize();
|
||||
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
|
||||
const Vector3 c2 = a2.cross(b2);
|
||||
b2CrossA1 = b2.cross(a1);
|
||||
mHingeJointComponents.mB2CrossA1[i] = b2CrossA1;
|
||||
c2CrossA1 = c2.cross(a1);
|
||||
mHingeJointComponents.mC2CrossA1[i] = c2CrossA1;
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
|
||||
|
||||
// Compute the impulse of body 1
|
||||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]);
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
|
||||
Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
|
||||
Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1;
|
||||
Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1;
|
||||
Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1;
|
||||
Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1;
|
||||
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
|
||||
b2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
|
||||
b2CrossA1.dot(I2C2CrossA1);
|
||||
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
|
||||
c2CrossA1.dot(I2B2CrossA1);
|
||||
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
|
||||
c2CrossA1.dot(I2C2CrossA1);
|
||||
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
|
||||
}
|
||||
|
||||
// Compute the position error for the 3 rotation constraints
|
||||
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse of body 2
|
||||
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
|
||||
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal lowerLimitError = hingeAngle - mHingeJointComponents.mLowerLimit[i];
|
||||
decimal upperLimitError = mHingeJointComponents.mUpperLimit[i] - hingeAngle;
|
||||
mHingeJointComponents.mIsLowerLimitViolated[i] = lowerLimitError <= 0;
|
||||
mHingeJointComponents.mIsUpperLimitViolated[i] = upperLimitError <= 0;
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mHingeJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i];
|
||||
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
||||
if (mHingeJointComponents.mIsLowerLimitViolated[i] || mHingeJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
|
||||
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / mHingeJointComponents.mInverseMassMatrixLimitMotor[i] : decimal(0.0);
|
||||
}
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mHingeJointComponents.mIsLowerLimitViolated[i]) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mHingeJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1;
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
|
||||
decimal SolveHingeJointSystem::computeNormalizedAngle(decimal angle) const {
|
||||
|
||||
// Convert it into the range [-2*pi; 2*pi]
|
||||
angle = std::fmod(angle, PI_TIMES_2);
|
||||
|
||||
// Convert it into the range [-pi; pi]
|
||||
if (angle < -PI) {
|
||||
return angle + PI_TIMES_2;
|
||||
}
|
||||
else if (angle > PI) {
|
||||
return angle - PI_TIMES_2;
|
||||
}
|
||||
else {
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
|
||||
// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
|
||||
// two angle limits in arguments.
|
||||
decimal SolveHingeJointSystem::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const {
|
||||
if (upperLimitAngle <= lowerLimitAngle) {
|
||||
return inputAngle;
|
||||
}
|
||||
else if (inputAngle > upperLimitAngle) {
|
||||
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 = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
|
||||
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
|
||||
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
|
||||
}
|
||||
else {
|
||||
return inputAngle;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal SolveHingeJointSystem::computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, const Quaternion& orientationBody2) {
|
||||
|
||||
decimal hingeAngle;
|
||||
|
||||
// Compute the current orientation difference between the two bodies
|
||||
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
|
||||
currentOrientationDiff.normalize();
|
||||
|
||||
// Compute the relative rotation considering the initial orientation difference
|
||||
Quaternion relativeRotation = currentOrientationDiff * mHingeJointComponents.getInitOrientationDifferenceInv(jointEntity);
|
||||
relativeRotation.normalize();
|
||||
|
||||
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
|
||||
// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
|
||||
// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
|
||||
// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
|
||||
// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
|
||||
// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
|
||||
// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
|
||||
decimal cosHalfAngle = relativeRotation.w;
|
||||
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
|
||||
|
||||
// Compute the dot product of the relative rotation axis and the hinge axis
|
||||
decimal dotProduct = relativeRotation.getVectorV().dot(mHingeJointComponents.getA1(jointEntity));
|
||||
|
||||
// If the relative rotation axis and the hinge axis are pointing the same direction
|
||||
if (dotProduct >= decimal(0.0)) {
|
||||
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
|
||||
}
|
||||
else {
|
||||
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
|
||||
}
|
||||
|
||||
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
|
||||
hingeAngle = computeNormalizedAngle(hingeAngle);
|
||||
|
||||
// Compute and return the corresponding angle near one the two limits
|
||||
return computeCorrespondingAngleNearLimits(hingeAngle,
|
||||
mHingeJointComponents.getLowerLimit(jointEntity),
|
||||
mHingeJointComponents.getUpperLimit(jointEntity));
|
||||
}
|
||||
160
src/systems/SolveHingeJointSystem.h
Normal file
160
src/systems/SolveHingeJointSystem.h
Normal file
|
|
@ -0,0 +1,160 @@
|
|||
/********************************************************************************
|
||||
* 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_SOLVE_HINGE_JOINT_SYSTEM_H
|
||||
#define REACTPHYSICS3D_SOLVE_HINGE_JOINT_SYSTEM_H
|
||||
|
||||
// Libraries
|
||||
#include "utils/Profiler.h"
|
||||
#include "components/RigidBodyComponents.h"
|
||||
#include "components/JointComponents.h"
|
||||
#include "components/HingeJointComponents.h"
|
||||
#include "components/TransformComponents.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
class DynamicsWorld;
|
||||
|
||||
// Class SolveHingeJointSystem
|
||||
/**
|
||||
* This class is responsible to solve the BallAndSocketJoint constraints
|
||||
*/
|
||||
class SolveHingeJointSystem {
|
||||
|
||||
private :
|
||||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
// Beta value for the bias factor of position correction
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
/// Reference to transform components
|
||||
TransformComponents& mTransformComponents;
|
||||
|
||||
/// Reference to the joint components
|
||||
JointComponents& mJointComponents;
|
||||
|
||||
/// Reference to the hinge joint components
|
||||
HingeJointComponents& mHingeJointComponents;
|
||||
|
||||
/// Current time step of the simulation
|
||||
decimal mTimeStep;
|
||||
|
||||
/// True if warm starting of the solver is active
|
||||
bool mIsWarmStartingActive;
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
/// Pointer to the profiler
|
||||
Profiler* mProfiler;
|
||||
#endif
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Given an angle in radian, this method returns the corresponding
|
||||
/// angle in the range [-pi; pi]
|
||||
decimal computeNormalizedAngle(decimal angle) const;
|
||||
|
||||
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
|
||||
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
|
||||
/// two angle limits in arguments.
|
||||
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
|
||||
decimal upperLimitAngle) const;
|
||||
|
||||
/// Compute the current angle around the hinge axis
|
||||
decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1,
|
||||
const Quaternion& orientationBody2);
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
SolveHingeJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
HingeJointComponents& hingeJointComponents);
|
||||
|
||||
/// Destructor
|
||||
~SolveHingeJointSystem() = default;
|
||||
|
||||
/// Initialize before solving the constraint
|
||||
void initBeforeSolve();
|
||||
|
||||
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void warmstart();
|
||||
|
||||
/// Solve the velocity constraint
|
||||
void solveVelocityConstraint();
|
||||
|
||||
/// Solve the position constraint (for position error correction)
|
||||
void solvePositionConstraint();
|
||||
|
||||
/// Set the time step
|
||||
void setTimeStep(decimal timeStep);
|
||||
|
||||
/// Set to true to enable warm starting
|
||||
void setIsWarmStartingActive(bool isWarmStartingActive);
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
/// Set the profiler
|
||||
void setProfiler(Profiler* profiler);
|
||||
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
#ifdef IS_PROFILING_ACTIVE
|
||||
|
||||
// Set the profiler
|
||||
inline void SolveHingeJointSystem::setProfiler(Profiler* profiler) {
|
||||
mProfiler = profiler;
|
||||
}
|
||||
|
||||
// Set the time step
|
||||
inline void SolveHingeJointSystem::setTimeStep(decimal timeStep) {
|
||||
assert(timeStep > decimal(0.0));
|
||||
mTimeStep = timeStep;
|
||||
}
|
||||
|
||||
// Set to true to enable warm starting
|
||||
inline void SolveHingeJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) {
|
||||
mIsWarmStartingActive = isWarmStartingActive;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user