resolve warning C4456: prefix each local version with "scope"
Signed-off-by: DNKpp <DNKpp2011@gmail.com>
This commit is contained in:
parent
c8de03cb8d
commit
9a0fc9eb18
@ -371,11 +371,11 @@ AABB CollisionBody::getAABB() const {
|
||||
// For each collider of the body
|
||||
for (uint i=1; i < colliderEntities.size(); i++) {
|
||||
|
||||
Collider* collider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]);
|
||||
Collider* scopeCollider = mWorld.mCollidersComponents.getCollider(colliderEntities[i]);
|
||||
|
||||
// Compute the world-space AABB of the collider
|
||||
AABB aabb;
|
||||
collider->getCollisionShape()->computeAABB(aabb, transform * collider->getLocalToBodyTransform());
|
||||
scopeCollider->getCollisionShape()->computeAABB(aabb, transform * scopeCollider->getLocalToBodyTransform());
|
||||
|
||||
// Merge the collider AABB with the current body AABB
|
||||
bodyAABB.mergeWithAABB(aabb);
|
||||
|
||||
@ -124,17 +124,17 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
|
||||
// Remove the previous contact point computed by GJK
|
||||
narrowPhaseInfoBatch.resetContactPoints(batchIndex);
|
||||
|
||||
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
|
||||
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
|
||||
const Transform scopeCapsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex] : narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex];
|
||||
const Transform polyhedronToCapsuleTransform = scopeCapsuleToWorld.getInverse() * polyhedronToWorld;
|
||||
|
||||
// Compute the end-points of the inner segment of the capsule
|
||||
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 scopeCapsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 scopeCapsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
|
||||
// Convert the inner capsule segment points into the polyhedron local-space
|
||||
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
|
||||
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
|
||||
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
|
||||
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * scopeCapsuleSegA;
|
||||
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * scopeCapsuleSegB;
|
||||
|
||||
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
|
||||
|
||||
|
||||
@ -1038,18 +1038,18 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
|
||||
itbodyContactPairs->second.add(newContactPairIndex);
|
||||
}
|
||||
else {
|
||||
List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
|
||||
contactPairs.add(newContactPairIndex);
|
||||
mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, contactPairs));
|
||||
List<uint> scopeContactPairs(mMemoryManager.getPoolAllocator(), 1);
|
||||
scopeContactPairs.add(newContactPairIndex);
|
||||
mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, scopeContactPairs));
|
||||
}
|
||||
itbodyContactPairs = mapBodyToContactPairs.find(body2Entity);
|
||||
if (itbodyContactPairs != mapBodyToContactPairs.end()) {
|
||||
itbodyContactPairs->second.add(newContactPairIndex);
|
||||
}
|
||||
else {
|
||||
List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
|
||||
contactPairs.add(newContactPairIndex);
|
||||
mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, contactPairs));
|
||||
List<uint> scopeContactPairs(mMemoryManager.getPoolAllocator(), 1);
|
||||
scopeContactPairs.add(newContactPairIndex);
|
||||
mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, scopeContactPairs));
|
||||
}
|
||||
}
|
||||
else { // If a ContactPair already exists for this overlapping pair, we use this one
|
||||
|
||||
@ -600,23 +600,23 @@ void ContactSolverSystem::solve() {
|
||||
deltaLambdaSplit, decimal(0.0));
|
||||
deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
|
||||
|
||||
Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
|
||||
Vector3 scopeLinearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
|
||||
mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit,
|
||||
mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * scopeLinearImpulse.x;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * scopeLinearImpulse.y;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * scopeLinearImpulse.z;
|
||||
|
||||
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
|
||||
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
|
||||
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * scopeLinearImpulse.x;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * scopeLinearImpulse.y;
|
||||
mRigidBodyComponents.mSplitLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * scopeLinearImpulse.z;
|
||||
|
||||
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
|
||||
mRigidBodyComponents.mSplitAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
|
||||
@ -643,24 +643,24 @@ void ContactSolverSystem::solve() {
|
||||
deltaV.z * mContactConstraints[c].frictionVector1.z;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
|
||||
decimal scopeDeltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
|
||||
decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].friction1Impulse;
|
||||
mContactConstraints[c].friction1Impulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].friction1Impulse +
|
||||
deltaLambda, frictionLimit));
|
||||
deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
|
||||
scopeDeltaLambda, frictionLimit));
|
||||
scopeDeltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.y * deltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.z * deltaLambda);
|
||||
Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda,
|
||||
mContactConstraints[c].frictionVector1.y * deltaLambda,
|
||||
mContactConstraints[c].frictionVector1.z * deltaLambda);
|
||||
Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.y * deltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.z * deltaLambda);
|
||||
Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * scopeDeltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.y * scopeDeltaLambda,
|
||||
-mContactConstraints[c].r1CrossT1.z * scopeDeltaLambda);
|
||||
Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * scopeDeltaLambda,
|
||||
mContactConstraints[c].frictionVector1.y * scopeDeltaLambda,
|
||||
mContactConstraints[c].frictionVector1.z * scopeDeltaLambda);
|
||||
Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * scopeDeltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.y * scopeDeltaLambda,
|
||||
mContactConstraints[c].r2CrossT1.z * scopeDeltaLambda);
|
||||
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
@ -691,26 +691,26 @@ void ContactSolverSystem::solve() {
|
||||
deltaV.z * mContactConstraints[c].frictionVector2.z;
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
|
||||
scopeDeltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
|
||||
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].friction2Impulse;
|
||||
mContactConstraints[c].friction2Impulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].friction2Impulse +
|
||||
deltaLambda, frictionLimit));
|
||||
deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
|
||||
scopeDeltaLambda, frictionLimit));
|
||||
scopeDeltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda;
|
||||
angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda;
|
||||
angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda;
|
||||
angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * scopeDeltaLambda;
|
||||
angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * scopeDeltaLambda;
|
||||
angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * scopeDeltaLambda;
|
||||
|
||||
linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda;
|
||||
linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda;
|
||||
linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda;
|
||||
linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * scopeDeltaLambda;
|
||||
linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * scopeDeltaLambda;
|
||||
linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * scopeDeltaLambda;
|
||||
|
||||
angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
|
||||
angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * scopeDeltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * scopeDeltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * scopeDeltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
|
||||
@ -731,18 +731,18 @@ void ContactSolverSystem::solve() {
|
||||
Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y +
|
||||
deltaV.z * mContactConstraints[c].normal.z;
|
||||
|
||||
deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
|
||||
scopeDeltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
|
||||
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
|
||||
lambdaTemp = mContactConstraints[c].frictionTwistImpulse;
|
||||
mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit,
|
||||
std::min(mContactConstraints[c].frictionTwistImpulse
|
||||
+ deltaLambda, frictionLimit));
|
||||
deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
|
||||
+ scopeDeltaLambda, frictionLimit));
|
||||
scopeDeltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
|
||||
angularImpulseBody2.x = mContactConstraints[c].normal.x * scopeDeltaLambda;
|
||||
angularImpulseBody2.y = mContactConstraints[c].normal.y * scopeDeltaLambda;
|
||||
angularImpulseBody2.z = mContactConstraints[c].normal.z * scopeDeltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
|
||||
|
||||
@ -456,16 +456,16 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
|
||||
deltaLambdaLower = mHingeJointComponents.mImpulseLowerLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
|
||||
const Vector3 scopeAngularImpulseBody1 = -deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
w1 += i1 * scopeAngularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * a1;
|
||||
const Vector3 scopeAngularImpulseBody2 = deltaLambdaLower * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
w2 += i2 * scopeAngularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
@ -481,16 +481,16 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
|
||||
deltaLambdaUpper = mHingeJointComponents.mImpulseUpperLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
|
||||
const Vector3 scopeAngularImpulseBody1 = deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
w1 += i1 * scopeAngularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1;
|
||||
const Vector3 scopeAngularImpulseBody2 = -deltaLambdaUpper * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
w2 += i2 * scopeAngularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
@ -510,16 +510,16 @@ void SolveHingeJointSystem::solveVelocityConstraint() {
|
||||
deltaLambdaMotor = mHingeJointComponents.mImpulseMotor[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
|
||||
const Vector3 scopeAngularImpulseBody1 = -deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
w1 += i1 * scopeAngularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1;
|
||||
const Vector3 scopeAngularImpulseBody2 = deltaLambdaMotor * a1;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
w2 += i2 * scopeAngularImpulseBody2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -892,29 +892,29 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
||||
decimal lambdaLowerLimit = mSliderJointComponents.mInverseMassMatrixLimit[i] * (-lowerLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis;
|
||||
const Vector3 scopeLinearImpulseBody1 = -lambdaLowerLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 scopeAngularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
const Vector3 v1 = inverseMassBody1 * scopeLinearImpulseBody1;
|
||||
const Vector3 scopeW1 = mSliderJointComponents.mI1[i] * scopeAngularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1 += Quaternion(0, scopeW1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis;
|
||||
const Vector3 scopeAngularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
const Vector3 scopeW2 = mSliderJointComponents.mI2[i] * scopeAngularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2 += Quaternion(0, scopeW2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
@ -932,28 +932,28 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis;
|
||||
const Vector3 scopeAngularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
const Vector3 scopeW1 = mSliderJointComponents.mI1[i] * scopeAngularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
x1 += v1;
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1 += Quaternion(0, scopeW1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis;
|
||||
const Vector3 scopeAngularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
const Vector3 scopeW2 = mSliderJointComponents.mI2[i] * scopeAngularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2 += Quaternion(0, scopeW2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user