diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index e0f45826..60ffd8a2 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -108,14 +108,13 @@ namespace SHADE memcpy_s(newConstraint.tangents, TANGENTS_SIZE, manifold.tangents, TANGENTS_SIZE); memcpy_s(newConstraint.contacts, CONTACTS_SIZE, manifold.contacts, CONTACTS_SIZE); - // Compute rA & rB for contacts with the local centroids - const SHVec3 LOCAL_COM_A = BODY_A ? BODY_A->localCentroid : SHVec3::Zero; - const SHVec3 LOCAL_COM_B = BODY_B ? BODY_B->localCentroid : SHVec3::Zero; + const SHCollisionUtils::ShapeTransform TF_A{ SHAPE_A->GetWorldCentroid(), SHAPE_A->GetWorldOrientation() }; + const SHCollisionUtils::ShapeTransform TF_B{ SHAPE_B->GetWorldCentroid(), SHAPE_B->GetWorldOrientation() }; for (uint32_t i = 0; i < newConstraint.numContacts; ++i) { - newConstraint.contacts[i].rA = newConstraint.contacts[i].localPointA - LOCAL_COM_A; - newConstraint.contacts[i].rB = newConstraint.contacts[i].localPointB - LOCAL_COM_B; + newConstraint.contacts[i].rA = (TF_A * newConstraint.contacts[i].localPointA) - newConstraint.centerOfMassA; + newConstraint.contacts[i].rB = (TF_B * newConstraint.contacts[i].localPointB) - newConstraint.centerOfMassB; } }