Fix issue with cone limit of ball-and-socket joint
This commit is contained in:
parent
4dae9449c9
commit
312a4cc9a8
|
@ -157,7 +157,7 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
|
|||
mBConeLimit[index] = decimal(0.0);
|
||||
mIsConeLimitViolated[index] = false;
|
||||
new (mConeLimitLocalAxisBody1 + index) Vector3(1, 0, 0);
|
||||
new (mConeLimitLocalAxisBody2 + index) Vector3(-1, 0, 0);
|
||||
new (mConeLimitLocalAxisBody2 + index) Vector3(1, 0, 0);
|
||||
new (mConeLimitACrossB + index) Vector3(0, 0, 0);
|
||||
|
||||
// Map the entity with the new component lookup index
|
||||
|
|
|
@ -118,8 +118,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
}
|
||||
|
||||
// Convert local-space cone axis of bodies to world-space
|
||||
const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex];
|
||||
const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex];
|
||||
const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
|
||||
const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
|
||||
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
|
||||
|
@ -400,8 +400,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
// Check if the cone limit constraints is violated or not
|
||||
const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex];
|
||||
const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex];
|
||||
const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
|
||||
const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
|
||||
|
@ -411,8 +411,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix)
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);
|
||||
|
||||
|
|
Loading…
Reference in New Issue
Block a user