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

Optimization of joints

Browse Source
This commit is contained in:
Daniel Chappuis 2020-10-12 23:01:57 +02:00
parent ec39d00a70
commit 916cefa96d
7 changed files with 273 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
include/reactphysics3d/body/RigidBody.h
View File

@ -212,6 +212,24 @@ class RigidBody : public CollisionBody {
friend class Joint;
};
/// Compute the inverse of the inertia tensor in world coordinates.
RP3D_FORCE_INLINE void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) {
outInverseInertiaTensorWorld[0][0] = orientation[0][0] * inverseInertiaTensorLocal.x;
outInverseInertiaTensorWorld[0][1] = orientation[1][0] * inverseInertiaTensorLocal.x;
outInverseInertiaTensorWorld[0][2] = orientation[2][0] * inverseInertiaTensorLocal.x;
outInverseInertiaTensorWorld[1][0] = orientation[0][1] * inverseInertiaTensorLocal.y;
outInverseInertiaTensorWorld[1][1] = orientation[1][1] * inverseInertiaTensorLocal.y;
outInverseInertiaTensorWorld[1][2] = orientation[2][1] * inverseInertiaTensorLocal.y;
outInverseInertiaTensorWorld[2][0] = orientation[0][2] * inverseInertiaTensorLocal.z;
outInverseInertiaTensorWorld[2][1] = orientation[1][2] * inverseInertiaTensorLocal.z;
outInverseInertiaTensorWorld[2][2] = orientation[2][2] * inverseInertiaTensorLocal.z;
outInverseInertiaTensorWorld = orientation * outInverseInertiaTensorWorld;
}
}
#endif

5
include/reactphysics3d/components/JointComponents.h
View File

@ -157,6 +157,11 @@ class JointComponents : public Components {
friend class BroadPhaseSystem;
friend class ConstraintSolverSystem;
friend class PhysicsWorld;
friend class SolveBallAndSocketJointSystem;
friend class SolveFixedJointSystem;
friend class SolveHingeJointSystem;
friend class SolveSliderJointSystem;
};
// Return the entity of the first body of a joint

10
src/body/RigidBody.cpp
View File

@ -888,16 +888,6 @@ void RigidBody::resetOverlappingPairs() {
askForBroadPhaseCollisionCheck();
}
/// Compute the inverse of the inertia tensor in world coordinates.
void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) {
Matrix3x3 orientationTranspose = orientation.getTranspose();
orientationTranspose[0] *= inverseInertiaTensorLocal.x;
orientationTranspose[1] *= inverseInertiaTensorLocal.y;
orientationTranspose[2] *= inverseInertiaTensorLocal.z;
outInverseInertiaTensorWorld = orientation * orientationTranspose;
}
// Set whether or not the body is allowed to go to sleep
/**
* @param isAllowedToSleep True if the body is allowed to sleep

84
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -48,13 +48,15 @@ SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(PhysicsWorld& world
void SolveBallAndSocketJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
@ -65,13 +67,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -82,9 +85,10 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Compute the corresponding skew-symmetric matrices
const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i];
@ -93,8 +97,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -122,13 +126,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
const decimal biasFactor = (BETA / mTimeStep);
// For each joint
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i];
const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i];
@ -138,7 +143,7 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
// Compute the bias "b" of the constraint
mBallAndSocketJointComponents.mBiasVector[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
}
}
@ -159,12 +164,14 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
void SolveBallAndSocketJointSystem::warmstart() {
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -202,12 +209,14 @@ void SolveBallAndSocketJointSystem::warmstart() {
void SolveBallAndSocketJointSystem::solveVelocityConstraint() {
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -249,16 +258,18 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() {
void SolveBallAndSocketJointSystem::solvePositionConstraint() {
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbEnabledJoints = mBallAndSocketJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
@ -271,16 +282,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the vector from body center to the anchor point in world-space
mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.getConstrainedOrientation(body1Entity) *
@ -290,16 +302,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -330,16 +343,17 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mBallAndSocketJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);

106
src/systems/SolveFixedJointSystem.cpp
View File

@ -48,13 +48,15 @@ SolveFixedJointSystem::SolveFixedJointSystem(PhysicsWorld& world, RigidBodyCompo
void SolveFixedJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
@ -65,13 +67,14 @@ void SolveFixedJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -82,13 +85,14 @@ void SolveFixedJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
@ -118,13 +122,14 @@ void SolveFixedJointSystem::initBeforeSolve() {
const decimal biasFactor = BETA / mTimeStep;
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Get the bodies positions and orientations
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
@ -135,19 +140,20 @@ void SolveFixedJointSystem::initBeforeSolve() {
// Compute the bias "b" of the constraint for the 3 translation constraints
mFixedJointComponents.mBiasTranslation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World);
}
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix)
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i];
@ -158,13 +164,14 @@ void SolveFixedJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the bias "b" for the 3 rotation constraints
mFixedJointComponents.mBiasRotation[i].setToZero();
@ -172,7 +179,7 @@ void SolveFixedJointSystem::initBeforeSolve() {
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
const Quaternion qError = orientationBody2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse();
mFixedJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV();
}
@ -182,7 +189,7 @@ void SolveFixedJointSystem::initBeforeSolve() {
if (!mIsWarmStartingActive) {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
// Reset the accumulated impulses
mFixedJointComponents.mImpulseTranslation[i].setToZero();
@ -195,13 +202,15 @@ void SolveFixedJointSystem::initBeforeSolve() {
void SolveFixedJointSystem::warmstart() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -253,13 +262,15 @@ void SolveFixedJointSystem::warmstart() {
void SolveFixedJointSystem::solveVelocityConstraint() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -334,17 +345,19 @@ void SolveFixedJointSystem::solveVelocityConstraint() {
void SolveFixedJointSystem::solvePositionConstraint() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbEnabledJoints = mFixedJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
@ -357,39 +370,41 @@ void SolveFixedJointSystem::solvePositionConstraint() {
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Get the bodies positions and orientations
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
mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.getLocalAnchorPointBody1(jointEntity);
mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.getLocalAnchorPointBody2(jointEntity);
mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i];
mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i];
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -422,17 +437,18 @@ void SolveFixedJointSystem::solvePositionConstraint() {
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];

122
src/systems/SolveHingeJointSystem.cpp
View File

@ -48,13 +48,15 @@ SolveHingeJointSystem::SolveHingeJointSystem(PhysicsWorld& world, RigidBodyCompo
void SolveHingeJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
@ -65,13 +67,14 @@ void SolveHingeJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -84,13 +87,14 @@ void SolveHingeJointSystem::initBeforeSolve() {
const decimal biasFactor = (BETA / mTimeStep);
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -109,19 +113,20 @@ void SolveHingeJointSystem::initBeforeSolve() {
// Compute the bias "b" of the rotation constraints
mHingeJointComponents.mBiasRotation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == 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++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
@ -148,13 +153,14 @@ void SolveHingeJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Get the bodies positions and orientations
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
@ -162,19 +168,20 @@ void SolveHingeJointSystem::initBeforeSolve() {
// Compute the bias "b" of the translation constraints
mHingeJointComponents.mBiasTranslation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == 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++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
@ -202,7 +209,7 @@ void SolveHingeJointSystem::initBeforeSolve() {
if (!mIsWarmStartingActive) {
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
// Reset all the accumulated impulses
mHingeJointComponents.mImpulseTranslation[i].setToZero();
@ -214,13 +221,14 @@ void SolveHingeJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -261,13 +269,13 @@ void SolveHingeJointSystem::initBeforeSolve() {
// Compute the bias "b" of the lower limit constraint
mHingeJointComponents.mBLowerLimit[i] = decimal(0.0);
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == 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) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
}
}
@ -279,13 +287,15 @@ void SolveHingeJointSystem::initBeforeSolve() {
void SolveHingeJointSystem::warmstart() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -357,13 +367,15 @@ void SolveHingeJointSystem::warmstart() {
void SolveHingeJointSystem::solveVelocityConstraint() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -528,16 +540,18 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
void SolveHingeJointSystem::solvePositionConstraint() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbEnabledJoints = mHingeJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// 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;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
@ -550,16 +564,17 @@ void SolveHingeJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// 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;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
@ -570,16 +585,17 @@ void SolveHingeJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// 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;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
@ -607,16 +623,17 @@ void SolveHingeJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// 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;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -730,16 +747,17 @@ void SolveHingeJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// 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;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);

122
src/systems/SolveSliderJointSystem.cpp
View File

@ -48,13 +48,15 @@ SolveSliderJointSystem::SolveSliderJointSystem(PhysicsWorld& world, RigidBodyCom
void SolveSliderJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
@ -65,13 +67,14 @@ void SolveSliderJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
@ -82,11 +85,12 @@ void SolveSliderJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
// Compute the two orthogonal vectors to the slider axis in world-space
@ -95,7 +99,7 @@ void SolveSliderJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector();
mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]);
@ -104,13 +108,14 @@ void SolveSliderJointSystem::initBeforeSolve() {
const decimal biasFactor = (BETA / mTimeStep);
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -147,7 +152,7 @@ void SolveSliderJointSystem::initBeforeSolve() {
// Compute the bias "b" of the translation constraint
mSliderJointComponents.mBiasTranslation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mSliderJointComponents.mBiasTranslation[i].x = u.dot(mSliderJointComponents.mN1[i]);
mSliderJointComponents.mBiasTranslation[i].y = u.dot(mSliderJointComponents.mN2[i]);
mSliderJointComponents.mBiasTranslation[i] *= biasFactor;
@ -173,20 +178,20 @@ void SolveSliderJointSystem::initBeforeSolve() {
// Compute the bias "b" of the lower limit constraint
mSliderJointComponents.mBLowerLimit[i] = decimal(0.0);
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mSliderJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mSliderJointComponents.mBUpperLimit[i] = decimal(0.0);
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
}
}
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
// Compute the cross products used in the Jacobians
mSliderJointComponents.mR2CrossN1[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN1[i]);
@ -195,13 +200,14 @@ void SolveSliderJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i];
const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i];
@ -250,20 +256,21 @@ void SolveSliderJointSystem::initBeforeSolve() {
}
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
// Compute the bias "b" of the rotation constraint
mSliderJointComponents.mBiasRotation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
const Quaternion qError = orientationBody2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse();
mSliderJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV();
}
@ -286,7 +293,7 @@ void SolveSliderJointSystem::initBeforeSolve() {
if (!mIsWarmStartingActive) {
// For each joint
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
// Reset all the accumulated impulses
mSliderJointComponents.mImpulseTranslation[i].setToZero();
@ -302,13 +309,15 @@ void SolveSliderJointSystem::initBeforeSolve() {
void SolveSliderJointSystem::warmstart() {
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -380,13 +389,15 @@ void SolveSliderJointSystem::warmstart() {
void SolveSliderJointSystem::solveVelocityConstraint() {
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -444,13 +455,14 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
}
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -482,13 +494,14 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
}
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -607,17 +620,19 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
void SolveSliderJointSystem::solvePositionConstraint() {
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
const uint32 nbEnabledJoints = mSliderJointComponents.getNbEnabledComponents();
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
@ -630,17 +645,18 @@ void SolveSliderJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
@ -651,17 +667,18 @@ void SolveSliderJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -776,17 +793,18 @@ void SolveSliderJointSystem::solvePositionConstraint() {
}
// For each joint component
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
for (uint32 i=0; i < nbEnabledJoints; i++) {
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);