From 312a4cc9a8717531f1a379a9520e0b9f63a1e06b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 4 Jul 2021 15:42:42 +0200 Subject: [PATCH] Fix issue with cone limit of ball-and-socket joint --- src/components/BallAndSocketJointComponents.cpp | 2 +- src/systems/SolveBallAndSocketJointSystem.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index d6366ffc..a7cf2017 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -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 diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index b7dd3584..6d8971bd 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -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);