From 10ad5647df8e79644f93fea4c731bdaf9f6c7ccc Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Thu, 12 Jan 2023 16:32:17 +0800 Subject: [PATCH] Improved stability of sphere vs polyhedron Still trying to solve the polyhedron vs polyhedron issue --- .../Narrowphase/SHSphereVsConvex.cpp | 8 +- .../src/Physics/Dynamics/SHContactManager.cpp | 5 +- .../src/Physics/Dynamics/SHContactSolver.cpp | 30 +++---- .../src/Physics/Dynamics/SHRigidBody.cpp | 61 +++++++++----- .../src/Physics/Dynamics/SHRigidBody.h | 82 +++++++++---------- 5 files changed, 107 insertions(+), 79 deletions(-) diff --git a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp index 5de8a10a..488070d6 100644 --- a/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp +++ b/SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp @@ -179,7 +179,13 @@ namespace SHADE bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept { - return SphereVsConvex(manifold, B, A); + const bool RESULT = SphereVsConvex(manifold, B, A); + if (RESULT) + { + // Flip the normal + manifold.normal = -manifold.normal; + } + return RESULT; } /*-----------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp index a13e47ff..bc391d4e 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp @@ -238,6 +238,7 @@ namespace SHADE void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept { + static const float SQRT_ONE_OVER_THREE = std::sqrtf(1.0f / 3.0f); // Early out since exiting a collision does not require an update beyond updating the state if (manifold.state == SHCollisionState::EXIT) return; @@ -250,13 +251,13 @@ namespace SHADE const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1]; // Compute tangents - if (std::fabs(NORMAL.x) >= SHMath::EULER_CONSTANT) + if (std::fabs(NORMAL.x) >= SQRT_ONE_OVER_THREE) tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f }; else tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y }; tangent0 = SHVec3::Normalise(tangent0); - tangent1 = SHVec3::Cross(NORMAL, tangent0); + tangent1 = SHVec3::Cross(tangent0, NORMAL); // Accumulate impulses for (uint32_t i = 0; i < manifold.numContacts; ++i) diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index fa6f1266..3bbfff37 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -147,9 +147,9 @@ namespace SHADE * | 0 0 iz || z | | iz * z | * * Then dot product the result with rXnT - * | ix * x |[ u v w ] - * | iy * y | = [ ix * x * w + iy * y * v + iz * z * w ] - * | iz * z | + * [ u v w ]| ix * x | + * | iy * y | = [ ix * x * u + iy * y * v + iz * z * w ] + * | iz * z | * * Simplified: * @@ -171,12 +171,12 @@ namespace SHADE for (int j = 0; j < SHContact::NUM_TANGENTS; ++j) { - const SHVec3 RA_CROSS_T = SHVec3::Cross(contact.rA, constraint.tangents[j]); - const SHVec3 RB_CROSS_T = SHVec3::Cross(contact.rB, constraint.tangents[j]); + const SHVec3 RA_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rA); + const SHVec3 RB_CROSS_T = SHVec3::Cross(constraint.tangents[j], contact.rB); contact.tangentMass[j] = INV_MASS_SUM; - contact.tangentMass[j] += SHVec3::Dot(RA_CROSS_T, constraint.invInertiaA * RA_CROSS_T); - contact.tangentMass[j] += SHVec3::Dot(RB_CROSS_T, constraint.invInertiaB * RB_CROSS_T); + contact.tangentMass[j] += SHVec3::Dot(constraint.invInertiaA * RA_CROSS_T, RA_CROSS_T); + contact.tangentMass[j] += SHVec3::Dot(constraint.invInertiaB * RB_CROSS_T, RB_CROSS_T); contact.tangentMass[j] = 1.0f / contact.tangentMass[j]; } @@ -187,15 +187,8 @@ namespace SHADE * restituion bias = restitution * (relative velocity /dot normal) */ - const SHVec3 RV_A = vA + SHVec3::Cross(wA, contact.rA); - const SHVec3 RV_B = vB + SHVec3::Cross(wB, contact.rB); - const float RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal); - const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP); - const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f; - - contact.bias = ERROR_BIAS + RESTITUTION_BIAS; - + // Warm starting // Compute impulses SHVec3 impulse = constraint.normal * contact.normalImpulse; @@ -208,6 +201,13 @@ namespace SHADE vB += impulse * constraint.invMassB * LINEAR_LOCK_B; wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B; + + const SHVec3 RV_A = vA + SHVec3::Cross(wA, contact.rA); + const SHVec3 RV_B = vB + SHVec3::Cross(wB, contact.rB); + const float RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal); + + const float RESTITUTION_BIAS = RV_N > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f; + contact.bias = ERROR_BIAS + RESTITUTION_BIAS; } velocityStateA.LinearVelocity = vA; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp index d76b79b8..57c2d90e 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp @@ -171,12 +171,12 @@ namespace SHADE return angularDrag; } - const SHMatrix& SHRigidBody::GetLocalInvInertia() const noexcept + const SHVec3& SHRigidBody::GetLocalInvInertia() const noexcept { return localInvInertia; } - const SHMatrix& SHRigidBody::GetWorldInvInertia() const noexcept + const SHVec3& SHRigidBody::GetWorldInvInertia() const noexcept { return worldInvInertia; } @@ -311,13 +311,13 @@ namespace SHADE if (bodyType != Type::DYNAMIC) { invMass = 0.0f; - localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = 0.0f; - worldInvInertia.m[0][0] = worldInvInertia.m[1][1] = worldInvInertia.m[2][2] = 0.0f; + localInvInertia = SHVec3::Zero; + worldInvInertia = SHVec3::Zero; } else { invMass = 1.0f; - localInvInertia = SHMatrix::Identity; + localInvInertia = SHVec3 { 1.0f }; } } @@ -550,7 +550,7 @@ namespace SHADE // Reset angular velocity along X-axis angularVelocity.x = 0.0f; // Set inertia tensor on the x-axis to 0 - localInvInertia.m[0][0] = 0.0f; + localInvInertia.x = 0.0f; } else { @@ -570,7 +570,7 @@ namespace SHADE // Reset angular velocity along Y-axis angularVelocity.y = 0.0f; // Set inertia tensor on the y-axis to 0 - localInvInertia.m[1][1] = 0.0f; + localInvInertia.y = 0.0f; } else { @@ -590,7 +590,7 @@ namespace SHADE // Reset angular velocity along Z-axis angularVelocity.z = 0.0f; // Set inertia tensor on the z-axis to 0 - localInvInertia.m[2][2] = 0.0f; + localInvInertia.z = 0.0f; } else { @@ -609,7 +609,7 @@ namespace SHADE return; accumulatedForce += force; - accumulatedTorque += SHVec3::Cross(pos, force); + angularVelocity += worldInvInertia * SHVec3::Cross(pos, force); } void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept @@ -640,25 +640,48 @@ namespace SHADE if (bodyType == Type::STATIC) return; - const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation); + const SHMatrix R = SHMatrix::Rotate(motionState.orientation); + const SHMatrix RT = SHMatrix::Transpose(R); - // Compute world inertia - worldInvInertia = SHMatrix::Transpose(ROTATION) * localInvInertia * ROTATION; + /* + Compute world inertia as a Vector + + | a b c || x | | ax + by + cz | + | d e f || y | = | dx + ey + fz | + | g h i || z | | gx + hy + iz | + */ + + SHMatrix tmp = SHMatrix + { + SHVec4 { localInvInertia.x, 0.0f, 0.0f, 0.0f } + , SHVec4 { 0.0f, localInvInertia.y, 0.0f, 0.0f } + , SHVec4 { 0.0f, 0.0f, localInvInertia.z, 0.0f } + , SHVec4 { 0.0f, 0.0f, 0.0, 1.0f } + }; + + tmp *= RT; + + worldInvInertia = SHVec3 + { + R.m[0][0] * tmp.m[0][0] + RT.m[0][1] * tmp.m[1][1] + RT.m[0][2] * tmp.m[2][2] + , R.m[1][0] * tmp.m[0][0] + RT.m[1][1] * tmp.m[1][1] + RT.m[1][2] * tmp.m[2][2] + , R.m[2][0] * tmp.m[0][0] + RT.m[2][1] * tmp.m[1][1] + RT.m[2][2] * tmp.m[2][2] + }; // Compute world centroid - worldCentroid = (ROTATION * localCentroid) + motionState.position; + worldCentroid = (R * localCentroid) + motionState.position; } void SHRigidBody::ComputeMassData() noexcept { // Reset centroid localCentroid = SHVec3::Zero; - localInvInertia = SHMatrix::Identity; + localInvInertia = SHVec3{ 1.0f }; // In the instance the body is a particle if (!collider || collider->GetCollisionShapes().empty()) { - localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = invMass; + localInvInertia = SHVec3{ invMass }; return; } @@ -736,11 +759,9 @@ namespace SHADE } // Set diagonals then invert - localInvInertia.m[0][0] = J.m[0][0]; - localInvInertia.m[1][1] = J.m[1][1]; - localInvInertia.m[2][2] = J.m[2][2]; - - localInvInertia = SHMatrix::Inverse(localInvInertia); + localInvInertia.x = totalMass == 0.0f ? 0.0f : 1.0f / J.m[0][0]; + localInvInertia.y = totalMass == 0.0f ? 0.0f : 1.0f / J.m[1][1]; + localInvInertia.z = totalMass == 0.0f ? 0.0f : 1.0f / J.m[2][2]; } /*-----------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h index 3c869ad2..936e7a1d 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h @@ -73,41 +73,41 @@ namespace SHADE /* Getter Functions */ /*-----------------------------------------------------------------------------------*/ - [[nodiscard]] Type GetType () const noexcept; + [[nodiscard]] Type GetType () const noexcept; - [[nodiscard]] float GetGravityScale () const noexcept; + [[nodiscard]] float GetGravityScale () const noexcept; - [[nodiscard]] float GetMass () const noexcept; - [[nodiscard]] float GetLinearDrag () const noexcept; - [[nodiscard]] float GetAngularDrag () const noexcept; + [[nodiscard]] float GetMass () const noexcept; + [[nodiscard]] float GetLinearDrag () const noexcept; + [[nodiscard]] float GetAngularDrag () const noexcept; - [[nodiscard]] const SHMatrix& GetLocalInvInertia () const noexcept; - [[nodiscard]] const SHMatrix& GetWorldInvInertia () const noexcept; + [[nodiscard]] const SHVec3& GetLocalInvInertia () const noexcept; + [[nodiscard]] const SHVec3& GetWorldInvInertia () const noexcept; - [[nodiscard]] const SHVec3& GetLocalCentroid () const noexcept; - [[nodiscard]] const SHVec3& GetWorldCentroid () const noexcept; + [[nodiscard]] const SHVec3& GetLocalCentroid () const noexcept; + [[nodiscard]] const SHVec3& GetWorldCentroid () const noexcept; - [[nodiscard]] const SHVec3& GetForce () const noexcept; - [[nodiscard]] const SHVec3& GetTorque () const noexcept; - [[nodiscard]] const SHVec3& GetLinearVelocity () const noexcept; - [[nodiscard]] const SHVec3& GetAngularVelocity () const noexcept; + [[nodiscard]] const SHVec3& GetForce () const noexcept; + [[nodiscard]] const SHVec3& GetTorque () const noexcept; + [[nodiscard]] const SHVec3& GetLinearVelocity () const noexcept; + [[nodiscard]] const SHVec3& GetAngularVelocity () const noexcept; // Flags - [[nodiscard]] bool IsActive () const noexcept; - [[nodiscard]] bool IsSleeping () const noexcept; - [[nodiscard]] bool IsSleepingEnabled () const noexcept; - [[nodiscard]] bool IsGravityEnabled () const noexcept; - [[nodiscard]] bool IsAutoMassEnabled () const noexcept; - [[nodiscard]] bool IsTriggerInMassData () const noexcept; - [[nodiscard]] bool GetFreezePositionX () const noexcept; - [[nodiscard]] bool GetFreezePositionY () const noexcept; - [[nodiscard]] bool GetFreezePositionZ () const noexcept; - [[nodiscard]] bool GetFreezeRotationX () const noexcept; - [[nodiscard]] bool GetFreezeRotationY () const noexcept; - [[nodiscard]] bool GetFreezeRotationZ () const noexcept; + [[nodiscard]] bool IsActive () const noexcept; + [[nodiscard]] bool IsSleeping () const noexcept; + [[nodiscard]] bool IsSleepingEnabled () const noexcept; + [[nodiscard]] bool IsGravityEnabled () const noexcept; + [[nodiscard]] bool IsAutoMassEnabled () const noexcept; + [[nodiscard]] bool IsTriggerInMassData () const noexcept; + [[nodiscard]] bool GetFreezePositionX () const noexcept; + [[nodiscard]] bool GetFreezePositionY () const noexcept; + [[nodiscard]] bool GetFreezePositionZ () const noexcept; + [[nodiscard]] bool GetFreezeRotationX () const noexcept; + [[nodiscard]] bool GetFreezeRotationY () const noexcept; + [[nodiscard]] bool GetFreezeRotationZ () const noexcept; - [[nodiscard]] SHMotionState& GetMotionState () noexcept; + [[nodiscard]] SHMotionState& GetMotionState () noexcept; /*-----------------------------------------------------------------------------------*/ /* Setter Functions */ @@ -228,28 +228,28 @@ namespace SHADE /*-----------------------------------------------------------------------------------*/ // The entityID here is only meant for linking with the actual component in the engine. - EntityID entityID; - SHCollider* collider; + EntityID entityID; + SHCollider* collider; - Type bodyType; + Type bodyType; - float gravityScale; + float gravityScale; - float invMass; - float linearDrag; - float angularDrag; + float invMass; + float linearDrag; + float angularDrag; - SHMatrix localInvInertia; - SHMatrix worldInvInertia; + SHVec3 localInvInertia; // Only store the diagonals + SHVec3 worldInvInertia; // Only store the diagonals - SHVec3 localCentroid; - SHVec3 worldCentroid; + SHVec3 localCentroid; + SHVec3 worldCentroid; - SHVec3 accumulatedForce; - SHVec3 accumulatedTorque; + SHVec3 accumulatedForce; + SHVec3 accumulatedTorque; - SHVec3 linearVelocity; - SHVec3 angularVelocity; + SHVec3 linearVelocity; + SHVec3 angularVelocity; // aZ aY aX pZ pY pX 0 0 0 addTriggersToMassData inIsland autoMass enableGravity enableSleeping sleeping active uint16_t flags;