From a49c674c2b445032ff2bb1f159c16b3c97abb728 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Wed, 4 Jan 2023 15:03:58 +0800 Subject: [PATCH] Generalised the Parallel Axis Theorem for computing inertia tensors --- Assets/Scenes/PhysicsSandbox.shade | 2 +- .../EditorWindow/MenuBar/SHEditorMenuBar.cpp | 34 +++++++-------- SHADE_Engine/src/Math/SHMatrix.cpp | 8 ++++ SHADE_Engine/src/Math/SHMatrix.h | 1 + SHADE_Engine/src/Math/Vector/SHVec3.cpp | 24 +++++++++++ SHADE_Engine/src/Math/Vector/SHVec3.h | 41 ++++++++++--------- .../src/Physics/Dynamics/SHRigidBody.cpp | 38 +++++++---------- 7 files changed, 87 insertions(+), 61 deletions(-) diff --git a/Assets/Scenes/PhysicsSandbox.shade b/Assets/Scenes/PhysicsSandbox.shade index 34e78cc3..6dea778f 100644 --- a/Assets/Scenes/PhysicsSandbox.shade +++ b/Assets/Scenes/PhysicsSandbox.shade @@ -77,7 +77,7 @@ Interpolate: true Sleeping Enabled: true Freeze Position X: false - Freeze Position Y: true + Freeze Position Y: false Freeze Position Z: false Freeze Rotation X: false Freeze Rotation Y: false diff --git a/SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp b/SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp index 5f9bf4d7..8e8c88f3 100644 --- a/SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp +++ b/SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp @@ -309,28 +309,28 @@ namespace SHADE void SHEditorMenuBar::DrawPhysicsSettings() noexcept { if (ImGui::BeginMenu("Physics Settings")) + { + if (auto* physicsDebugDraw = SHSystemManager::GetSystem()) { - if (auto* physicsDebugDraw = SHSystemManager::GetSystem()) - { - bool drawColliders = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS); - if (ImGui::Checkbox("Draw Colliders", &drawColliders)) - physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS, drawColliders); + bool drawColliders = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS); + if (ImGui::Checkbox("Draw Colliders", &drawColliders)) + physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS, drawColliders); - bool drawContactPoints = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS); - if (ImGui::Checkbox("Draw Contact Points", &drawContactPoints)) - physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS, drawContactPoints); + bool drawContactPoints = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS); + if (ImGui::Checkbox("Draw Contact Points", &drawContactPoints)) + physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS, drawContactPoints); - bool drawRays = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS); - if (ImGui::Checkbox("Draw Rays", &drawRays)) - physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays); + bool drawRays = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS); + if (ImGui::Checkbox("Draw Rays", &drawRays)) + physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays); - bool drawBroadphase = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE); - if (ImGui::Checkbox("Draw Broadphase", &drawBroadphase)) - physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE, drawBroadphase); - } - - ImGui::EndMenu(); + bool drawBroadphase = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE); + if (ImGui::Checkbox("Draw Broadphase", &drawBroadphase)) + physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE, drawBroadphase); } + + ImGui::EndMenu(); + } } }//namespace SHADE diff --git a/SHADE_Engine/src/Math/SHMatrix.cpp b/SHADE_Engine/src/Math/SHMatrix.cpp index 3d450a88..2cbd5ef6 100644 --- a/SHADE_Engine/src/Math/SHMatrix.cpp +++ b/SHADE_Engine/src/Math/SHMatrix.cpp @@ -34,6 +34,14 @@ namespace SHADE 0.0f, 0.0f, 0.0f, 1.0f }; + const SHMatrix SHMatrix::Zero + { + SHVec4::Zero + , SHVec4::Zero + , SHVec4::Zero + , SHVec4::Zero + }; + /*-----------------------------------------------------------------------------------*/ /* Constructors & Destructor Definitions */ /*-----------------------------------------------------------------------------------*/ diff --git a/SHADE_Engine/src/Math/SHMatrix.h b/SHADE_Engine/src/Math/SHMatrix.h index 6af8fdc9..ffd8ce89 100644 --- a/SHADE_Engine/src/Math/SHMatrix.h +++ b/SHADE_Engine/src/Math/SHMatrix.h @@ -45,6 +45,7 @@ namespace SHADE static constexpr size_t NUM_COLS = 4U; static const SHMatrix Identity; + static const SHMatrix Zero; /*---------------------------------------------------------------------------------*/ /* Constructors & Destructor */ diff --git a/SHADE_Engine/src/Math/Vector/SHVec3.cpp b/SHADE_Engine/src/Math/Vector/SHVec3.cpp index 6b042c61..72d2b0a2 100644 --- a/SHADE_Engine/src/Math/Vector/SHVec3.cpp +++ b/SHADE_Engine/src/Math/Vector/SHVec3.cpp @@ -372,6 +372,30 @@ namespace SHADE return lhs.Cross(rhs); } + SHMatrix SHVec3::OuterProduct(const SHVec3& lhs, const SHVec3& rhs) noexcept + { + /* + * Outer product is a matrix multiplication u * vT + * 3x1 * 1x3 = 3x3 + * + * | u1 | | v1 v2 v3 | | u1v1 u1v2 u1v3 | + * | u2 | = | u2v1 u2v2 u2v3 | + * | u3 | | u3v1 u3v2 u3v3 | + */ + + SHMatrix u = SHMatrix::Zero; + SHMatrix vT = SHMatrix::Zero; + + for (int i = 0; i < SIZE; ++i) + { + u.m[0][i] = lhs[i]; + vT.m[i][0] = rhs[i]; + } + + return u * vT; + } + + SHVec3 SHVec3::Project(const SHVec3& v, const SHVec3& u) noexcept { SHVec3 result; diff --git a/SHADE_Engine/src/Math/Vector/SHVec3.h b/SHADE_Engine/src/Math/Vector/SHVec3.h index 657e167e..4b0112c5 100644 --- a/SHADE_Engine/src/Math/Vector/SHVec3.h +++ b/SHADE_Engine/src/Math/Vector/SHVec3.h @@ -115,27 +115,28 @@ namespace SHADE /* Static Function Members */ /*---------------------------------------------------------------------------------*/ - [[nodiscard]] static SHVec3 Normalise (const SHVec3& v) noexcept; - [[nodiscard]] static SHVec3 Abs (const SHVec3& v) noexcept; - [[nodiscard]] static SHVec3 Min (const std::initializer_list& vs) noexcept; - [[nodiscard]] static SHVec3 Max (const std::initializer_list& vs) noexcept; - [[nodiscard]] static SHVec3 Clamp (const SHVec3& v, const SHVec3& vMin, const SHVec3& vMax) noexcept; - [[nodiscard]] static SHVec3 Lerp (const SHVec3& a, const SHVec3& b, float t) noexcept; - [[nodiscard]] static SHVec3 ClampedLerp (const SHVec3& a, const SHVec3& b, float t, float tMin = 0.0f, float tMax = 1.0f) noexcept; + [[nodiscard]] static SHVec3 Normalise (const SHVec3& v) noexcept; + [[nodiscard]] static SHVec3 Abs (const SHVec3& v) noexcept; + [[nodiscard]] static SHVec3 Min (const std::initializer_list& vs) noexcept; + [[nodiscard]] static SHVec3 Max (const std::initializer_list& vs) noexcept; + [[nodiscard]] static SHVec3 Clamp (const SHVec3& v, const SHVec3& vMin, const SHVec3& vMax) noexcept; + [[nodiscard]] static SHVec3 Lerp (const SHVec3& a, const SHVec3& b, float t) noexcept; + [[nodiscard]] static SHVec3 ClampedLerp (const SHVec3& a, const SHVec3& b, float t, float tMin = 0.0f, float tMax = 1.0f) noexcept; - [[nodiscard]] static float Distance (const SHVec3& lhs, const SHVec3& rhs) noexcept; - [[nodiscard]] static float DistanceSquared (const SHVec3& lhs, const SHVec3& rhs) noexcept; - [[nodiscard]] static float Angle (const SHVec3& lhs, const SHVec3& rhs) noexcept; - [[nodiscard]] static float Dot (const SHVec3& lhs, const SHVec3& rhs) noexcept; - [[nodiscard]] static SHVec3 Cross (const SHVec3& lhs, const SHVec3& rhs) noexcept; - [[nodiscard]] static SHVec3 Project (const SHVec3& v, const SHVec3& u) noexcept; - [[nodiscard]] static SHVec3 Reflect (const SHVec3& v, const SHVec3& normal) noexcept; - [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHVec3& axis, float angleInRad) noexcept; - [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHQuaternion& q) noexcept; - [[nodiscard]] static SHVec3 RotateX (const SHVec3& v, float angleInRad) noexcept; - [[nodiscard]] static SHVec3 RotateY (const SHVec3& v, float angleInRad) noexcept; - [[nodiscard]] static SHVec3 RotateZ (const SHVec3& v, float angleInRad) noexcept; - [[nodiscard]] static SHVec3 Transform (const SHVec3& v, const SHMatrix& transformMtx) noexcept; + [[nodiscard]] static float Distance (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static float DistanceSquared (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static float Angle (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static float Dot (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static SHVec3 Cross (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static SHMatrix OuterProduct (const SHVec3& lhs, const SHVec3& rhs) noexcept; + [[nodiscard]] static SHVec3 Project (const SHVec3& v, const SHVec3& u) noexcept; + [[nodiscard]] static SHVec3 Reflect (const SHVec3& v, const SHVec3& normal) noexcept; + [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHVec3& axis, float angleInRad) noexcept; + [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHQuaternion& q) noexcept; + [[nodiscard]] static SHVec3 RotateX (const SHVec3& v, float angleInRad) noexcept; + [[nodiscard]] static SHVec3 RotateY (const SHVec3& v, float angleInRad) noexcept; + [[nodiscard]] static SHVec3 RotateZ (const SHVec3& v, float angleInRad) noexcept; + [[nodiscard]] static SHVec3 Transform (const SHVec3& v, const SHMatrix& transformMtx) noexcept; }; SHVec3 operator* (float lhs, const SHVec3& rhs) noexcept; diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp index c4b27a42..3b2de659 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp @@ -14,6 +14,7 @@ #include "SHRigidBody.h" // Project Headers +#include #include #include "Physics/Collision/SHCollider.h" @@ -664,26 +665,24 @@ namespace SHADE const float CUSTOM_MASS = 1.0f / invMass; const bool AUTO_MASS = IsAutoMassEnabled(); const bool INCLUDE_TRIGGERS = IsTriggerInMassData(); - const auto& SHAPES = collider->GetCollisionShapes(); + // Compute Total mass and store individual masses if custom mass is being used. // Compute local centroid at the same time // Zero matrix; - SHMatrix tmpLocalTensor; - tmpLocalTensor -= SHMatrix::Identity; + SHMatrix J = SHMatrix::Zero; float totalMass = 0.0f; - std::vector trueMass; + std::vector trueMass; // We store the true masses here for calculating the ratio with custom masses. + const auto& SHAPES = collider->GetCollisionShapes(); for (auto* shape : SHAPES) { // We skip triggers by default if (shape->IsTrigger() && !INCLUDE_TRIGGERS) continue; - shape->ComputeTransforms(); - // p = m/v, therefore m = pv. This is the true mass of the shape. const float MASS = shape->GetDensity() * shape->GetVolume(); totalMass += MASS; @@ -705,6 +704,7 @@ namespace SHADE const SHMatrix R = SHMatrix::Rotate(motionState.orientation); const SHMatrix RT = SHMatrix::Transpose(R); + // We need the world centroid to compute the offset of the collider from the body's centroid worldCentroid = (R * localCentroid) + motionState.position; for (size_t i = 0; i < SHAPES.size(); ++i) @@ -721,31 +721,23 @@ namespace SHADE actualMass *= CUSTOM_MASS / totalMass; // Convert inertia tensor into local-space of the body + // R * I * RT = R * (I * RT) SHMatrix I = SHAPE->GetInertiaTensor( actualMass ) * RT; I = R * I; // Parallel Axis Theorem - const SHVec3 O = SHAPE->GetPosition() - worldCentroid; - const float O2 = O.LengthSquared(); + // J = I + m((R /dot R)E_3 - R /outerProduct R) + const SHVec3 R = SHAPE->GetPosition() - worldCentroid; + const float R_MAG2 = R.LengthSquared(); + const SHMatrix R_OX_R = SHVec3::OuterProduct(R, R); - SHMatrix offsetMatrix; - offsetMatrix -= SHMatrix::Identity; - offsetMatrix.m[0][0] = offsetMatrix.m[1][1] = offsetMatrix.m[2][2] = O2; - - const SHVec3 NOX = O * -O.x; - const SHVec3 NOY = O * -O.y; - const SHVec3 NOZ = O * -O.z; - - offsetMatrix += SHMatrix{ NOX, NOY, NOZ, SHVec4::Zero }; - offsetMatrix *= actualMass; - - tmpLocalTensor += I + offsetMatrix; + J += I + actualMass * (SHMatrix::Identity * R_MAG2 - R_OX_R); } // Set diagonals then invert - localInvInertia.m[0][0] = tmpLocalTensor.m[0][0]; - localInvInertia.m[1][1] = tmpLocalTensor.m[1][1]; - localInvInertia.m[2][2] = tmpLocalTensor.m[2][2]; + 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); }