Optimization of joints solver
This commit is contained in:
Daniel Chappuis
parent
916cefa96d
commit
2078971331
|
|
@ -47,6 +47,8 @@ SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(PhysicsWorld& world
|
|||
// Initialize before solving the constraint
|
||||
void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
const uint32 nbJoints = mBallAndSocketJointComponents.getNbEnabledComponents();
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
|
@ -64,17 +66,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
// Get the inertia tensor of bodies
|
||||
mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
|
@ -82,13 +73,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
// Compute the vector from body center to the anchor point in world-space
|
||||
mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i];
|
||||
mBallAndSocketJointComponents.mR2World[i] = orientationBody2 * mBallAndSocketJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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];
|
||||
|
|
@ -96,10 +80,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
|
||||
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
|
|
@ -121,22 +101,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i];
|
||||
const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i];
|
||||
|
||||
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
|
||||
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity);
|
||||
|
|
@ -146,14 +110,10 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
// Reset the accumulated impulse
|
||||
mBallAndSocketJointComponents.mImpulse[i].setToZero();
|
||||
}
|
||||
|
|
@ -271,51 +231,23 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
|
||||
mBallAndSocketJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
|
||||
mBallAndSocketJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
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) *
|
||||
mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] *
|
||||
mBallAndSocketJointComponents.mLocalAnchorPointBody1[i];
|
||||
mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.getConstrainedOrientation(body2Entity) *
|
||||
mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] *
|
||||
mBallAndSocketJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i];
|
||||
const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i];
|
||||
|
|
@ -340,30 +272,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i];
|
||||
const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i];
|
||||
|
||||
// Compute the constraint error (value of the C(x) function)
|
||||
const Vector3 constraintError = (x2 + r2World - x1 - r1World);
|
||||
|
||||
|
|
@ -376,10 +288,6 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
const Vector3 linearImpulseBody1 = -lambda;
|
||||
const Vector3 angularImpulseBody1 = lambda.cross(r1World);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
|
|
|||
|
|
@ -47,6 +47,8 @@ SolveFixedJointSystem::SolveFixedJointSystem(PhysicsWorld& world, RigidBodyCompo
|
|||
// Initialize before solving the constraint
|
||||
void SolveFixedJointSystem::initBeforeSolve() {
|
||||
|
||||
const decimal biasFactor = BETA / mTimeStep;
|
||||
|
||||
// For each joint
|
||||
const uint32 nbJoints = mFixedJointComponents.getNbEnabledComponents();
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
|
@ -64,17 +66,6 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
// Get the inertia tensor of bodies
|
||||
mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
|
@ -82,17 +73,6 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
// Compute the vector from body center to the anchor point in world-space
|
||||
mFixedJointComponents.mR1World[i] = orientationBody1 * mFixedJointComponents.mLocalAnchorPointBody1[i];
|
||||
mFixedJointComponents.mR2World[i] = orientationBody2 * mFixedJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
// Compute the corresponding skew-symmetric matrices
|
||||
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
|
||||
|
|
@ -117,19 +97,6 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
const decimal biasFactor = BETA / mTimeStep;
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
|
||||
|
|
@ -143,17 +110,6 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World);
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.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];
|
||||
|
|
@ -161,36 +117,18 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
// Compute the bias "b" for the 3 rotation constraints
|
||||
mFixedJointComponents.mBiasRotation[i].setToZero();
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
||||
// Reset the accumulated impulses
|
||||
mFixedJointComponents.mImpulseTranslation[i].setToZero();
|
||||
mFixedJointComponents.mImpulseRotation[i].setToZero();
|
||||
|
|
@ -367,47 +305,17 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
mFixedJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Compute the vector from body center to the anchor point in world-space
|
||||
mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i];
|
||||
mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
|
|
@ -434,33 +342,9 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Vector3& r1World = mFixedJointComponents.mR1World[i];
|
||||
const Vector3& r2World = mFixedJointComponents.mR2World[i];
|
||||
|
||||
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];
|
||||
|
||||
// Compute position error for the 3 translation constraints
|
||||
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
|
||||
|
||||
|
|
@ -471,8 +355,6 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
Vector3 linearImpulseBody1 = -lambdaTranslation;
|
||||
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
|
||||
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
Vector3 w1 = mFixedJointComponents.mI2[i] * angularImpulseBody1;
|
||||
|
|
@ -485,8 +367,6 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
// Compute the impulse of body 2
|
||||
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
|
||||
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
|
||||
Vector3 w2 = mFixedJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
|
|
|||
|
|
@ -47,6 +47,8 @@ SolveHingeJointSystem::SolveHingeJointSystem(PhysicsWorld& world, RigidBodyCompo
|
|||
// Initialize before solving the constraint
|
||||
void SolveHingeJointSystem::initBeforeSolve() {
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
const uint32 nbJoints = mHingeJointComponents.getNbEnabledComponents();
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
|
@ -58,23 +60,15 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody1]);
|
||||
assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody2]);
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mHingeJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mHingeJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
mHingeJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1];
|
||||
mHingeJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2];
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
|
@ -82,22 +76,6 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
// 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 < nbJoints; i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
|
||||
|
||||
// Get the bodies entities
|
||||
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 vectors needed in the Jacobian
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
|
|
@ -116,25 +94,11 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
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 < nbJoints; i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
|
||||
|
||||
// Get the bodies entities
|
||||
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]);
|
||||
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];
|
||||
|
|
@ -150,38 +114,16 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
// Get the bodies positions and orientations
|
||||
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
|
||||
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity);
|
||||
const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
|
||||
const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2];
|
||||
|
||||
// Compute the bias "b" of the translation constraints
|
||||
mHingeJointComponents.mBiasTranslation[i].setToZero();
|
||||
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 < nbJoints; i++) {
|
||||
|
||||
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
|
||||
const uint32 jointIndex = mJointComponents.getEntityIndex(jointEntity);
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
|
||||
|
|
@ -199,18 +141,14 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
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) {
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
mHingeJointComponents.mImpulseTranslation[i].setToZero();
|
||||
mHingeJointComponents.mImpulseRotation[i].setToZero();
|
||||
|
|
@ -218,20 +156,6 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
|
||||
mHingeJointComponents.mImpulseMotor[i] = decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
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);
|
||||
|
|
@ -553,49 +477,24 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
|
||||
mHingeJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
|
||||
mHingeJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// 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 < 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
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]);
|
||||
|
|
@ -603,9 +502,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
|
||||
// --------------- 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];
|
||||
|
|
@ -620,26 +516,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
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];
|
||||
|
|
@ -744,23 +620,6 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
// 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 < 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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -47,6 +47,8 @@ SolveSliderJointSystem::SolveSliderJointSystem(PhysicsWorld& world, RigidBodyCom
|
|||
// Initialize before solving the constraint
|
||||
void SolveSliderJointSystem::initBeforeSolve() {
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
const uint32 nbJoints = mSliderJointComponents.getNbEnabledComponents();
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
|
@ -58,23 +60,15 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody1]);
|
||||
assert(!mRigidBodyComponents.mIsEntityDisabled[componentIndexBody2]);
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mSliderJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mSliderJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
mSliderJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1];
|
||||
mSliderJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2];
|
||||
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
|
@ -82,43 +76,13 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
// Vector from body center to the anchor point
|
||||
mSliderJointComponents.mR1[i] = orientationBody1 * mSliderJointComponents.mLocalAnchorPointBody1[i];
|
||||
mSliderJointComponents.mR2[i] = orientationBody2 * mSliderJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
|
||||
const uint32 jointIndex = mJointComponents.getEntityIndex(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
|
||||
mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i];
|
||||
mSliderJointComponents.mSliderAxisWorld[i].normalize();
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
||||
mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector();
|
||||
mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]);
|
||||
}
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
|
||||
const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2];
|
||||
|
|
@ -188,26 +152,11 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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]);
|
||||
mSliderJointComponents.mR2CrossN2[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN2[i]);
|
||||
mSliderJointComponents.mR2CrossSliderAxis[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i];
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i];
|
||||
|
|
@ -217,9 +166,6 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
const Matrix3x3& i1 = mSliderJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mSliderJointComponents.mI2[i];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
|
|
@ -253,20 +199,6 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
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.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();
|
||||
|
|
@ -278,8 +210,8 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
// If the motor is enabled
|
||||
if (mSliderJointComponents.mIsMotorEnabled[i]) {
|
||||
|
||||
const decimal body1MassInverse = mRigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal body2MassInverse = mRigidBodyComponents.getMassInverse(body2Entity);
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
|
||||
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||
|
|
@ -287,14 +219,10 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
mSliderJointComponents.mInverseMassMatrixMotor[i] = (mSliderJointComponents.mInverseMassMatrixMotor[i] > decimal(0.0)) ?
|
||||
decimal(1.0) / mSliderJointComponents.mInverseMassMatrixMotor[i] : decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < nbJoints; i++) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
mSliderJointComponents.mImpulseTranslation[i].setToZero();
|
||||
mSliderJointComponents.mImpulseRotation[i].setToZero();
|
||||
|
|
@ -452,26 +380,9 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
|
|||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
|
|
@ -481,36 +392,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
|
|||
mSliderJointComponents.mImpulseRotation[i] += deltaLambda2;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -deltaLambda2;
|
||||
angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body to body 1
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = deltaLambda2;
|
||||
angularImpulseBody2 = deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
|
||||
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
|
||||
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
|
@ -634,54 +525,24 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
|
||||
mSliderJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
|
||||
mSliderJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mSliderJointComponents.mR1[i] = q1 * mSliderJointComponents.mLocalAnchorPointBody1[i];
|
||||
mSliderJointComponents.mR2[i] = q2 * mSliderJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
|
|
@ -699,9 +560,6 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
// Compute the vector u (difference between anchor points)
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i];
|
||||
mSliderJointComponents.mSliderAxisWorld[i].normalize();
|
||||
|
|
@ -790,31 +648,6 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
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.mPositionCorrectionTechniques[jointIndex] != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
|
||||
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Get the velocities
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
|
|
@ -858,7 +691,7 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
Vector3 lambdaRotation = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -lambdaRotation;
|
||||
angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
|
@ -868,7 +701,7 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = lambdaRotation;
|
||||
angularImpulseBody2 = lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user