xiaoqi
/
SHADE_Y3
1
0
Fork 0
Code Issues 12 Pull Requests Packages Projects Releases 9 Wiki Activity

Generalised the Parallel Axis Theorem for computing inertia tensors

This commit is contained in:
Diren D Bharwani 2023-01-04 15:03:58 +08:00
parent d7fa40776a
commit a49c674c2b
7 changed files with 87 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Assets/Scenes/PhysicsSandbox.shade
View File

@ -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

34
SHADE_Engine/src/Editor/EditorWindow/MenuBar/SHEditorMenuBar.cpp
View File

@ -309,28 +309,28 @@ namespace SHADE
void SHEditorMenuBar::DrawPhysicsSettings() noexcept
{
if (ImGui::BeginMenu("Physics Settings"))
{
if (auto* physicsDebugDraw = SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>())
{
if (auto* physicsDebugDraw = SHSystemManager::GetSystem<SHPhysicsDebugDrawSystem>())
{
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

8
SHADE_Engine/src/Math/SHMatrix.cpp
View File

@ -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 */
/*-----------------------------------------------------------------------------------*/

1
SHADE_Engine/src/Math/SHMatrix.h
View File

@ -45,6 +45,7 @@ namespace SHADE
static constexpr size_t NUM_COLS = 4U;
static const SHMatrix Identity;
static const SHMatrix Zero;
/*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */

24
SHADE_Engine/src/Math/Vector/SHVec3.cpp
View File

@ -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;

41
SHADE_Engine/src/Math/Vector/SHVec3.h
View File

@ -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<SHVec3>& vs) noexcept;
[[nodiscard]] static SHVec3 Max (const std::initializer_list<SHVec3>& 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<SHVec3>& vs) noexcept;
[[nodiscard]] static SHVec3 Max (const std::initializer_list<SHVec3>& 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;

38
SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp
View File

@ -14,6 +14,7 @@
#include "SHRigidBody.h"
// Project Headers
#include <complex.h>
#include <numeric>
#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<float> trueMass;
std::vector<float> 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);
}