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

Implemented a custom physics engine #316

Merged
direnbharwani merged 95 commits from SHPhysics into main 2023-01-23 15:55:45 +08:00
Conversation 0 Commits 95 Files Changed 144 +87 -61
7 changed files with 87 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a49c674c2b - Show all commits

2
Assets/Scenes/PhysicsSandbox.shade
View File

@ -77,7 +77,7 @@
Interpolate: true Interpolate: true
Sleeping Enabled: true Sleeping Enabled: true
Freeze Position X: false Freeze Position X: false
Freeze Position Y: true Freeze Position Y: false
Freeze Position Z: false Freeze Position Z: false
Freeze Rotation X: false Freeze Rotation X: false
Freeze Rotation Y: 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 void SHEditorMenuBar::DrawPhysicsSettings() noexcept
{ {
if (ImGui::BeginMenu("Physics Settings")) 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))
bool drawColliders = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS); physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS, drawColliders);
if (ImGui::Checkbox("Draw Colliders", &drawColliders))
physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::COLLIDERS, drawColliders);
bool drawContactPoints = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS); bool drawContactPoints = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS);
if (ImGui::Checkbox("Draw Contact Points", &drawContactPoints)) if (ImGui::Checkbox("Draw Contact Points", &drawContactPoints))
physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS, drawContactPoints); physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::CONTACTS, drawContactPoints);
bool drawRays = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS); bool drawRays = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS);
if (ImGui::Checkbox("Draw Rays", &drawRays)) if (ImGui::Checkbox("Draw Rays", &drawRays))
physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays); physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::RAYCASTS, drawRays);
bool drawBroadphase = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE); bool drawBroadphase = physicsDebugDraw->GetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE);
if (ImGui::Checkbox("Draw Broadphase", &drawBroadphase)) if (ImGui::Checkbox("Draw Broadphase", &drawBroadphase))
physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE, drawBroadphase); physicsDebugDraw->SetFlagState(SHPhysicsDebugDrawSystem::DebugDrawFlags::BROADPHASE, drawBroadphase);
}
ImGui::EndMenu();
} }
ImGui::EndMenu();
}
} }
}//namespace SHADE }//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 0.0f, 0.0f, 0.0f, 1.0f
}; };
const SHMatrix SHMatrix::Zero
{
SHVec4::Zero
, SHVec4::Zero
, SHVec4::Zero
, SHVec4::Zero
};
/*-----------------------------------------------------------------------------------*/ /*-----------------------------------------------------------------------------------*/
/* Constructors & Destructor Definitions */ /* 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 constexpr size_t NUM_COLS = 4U;
static const SHMatrix Identity; static const SHMatrix Identity;
static const SHMatrix Zero;
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */ /* Constructors & Destructor */

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

@ -372,6 +372,30 @@ namespace SHADE
return lhs.Cross(rhs); 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 SHVec3::Project(const SHVec3& v, const SHVec3& u) noexcept
{ {
SHVec3 result; SHVec3 result;

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

@ -115,27 +115,28 @@ namespace SHADE
/* Static Function Members */ /* Static Function Members */
/*---------------------------------------------------------------------------------*/ /*---------------------------------------------------------------------------------*/
[[nodiscard]] static SHVec3 Normalise (const SHVec3& v) noexcept; [[nodiscard]] static SHVec3 Normalise (const SHVec3& v) noexcept;
[[nodiscard]] static SHVec3 Abs (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 Min (const std::initializer_list<SHVec3>& vs) noexcept;
[[nodiscard]] static SHVec3 Max (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 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 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 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 Distance (const SHVec3& lhs, const SHVec3& rhs) noexcept;
[[nodiscard]] static float DistanceSquared (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 Angle (const SHVec3& lhs, const SHVec3& rhs) noexcept;
[[nodiscard]] static float Dot (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 Cross (const SHVec3& lhs, const SHVec3& rhs) noexcept;
[[nodiscard]] static SHVec3 Project (const SHVec3& v, const SHVec3& u) noexcept; [[nodiscard]] static SHMatrix OuterProduct (const SHVec3& lhs, const SHVec3& rhs) noexcept;
[[nodiscard]] static SHVec3 Reflect (const SHVec3& v, const SHVec3& normal) noexcept; [[nodiscard]] static SHVec3 Project (const SHVec3& v, const SHVec3& u) noexcept;
[[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHVec3& axis, float angleInRad) noexcept; [[nodiscard]] static SHVec3 Reflect (const SHVec3& v, const SHVec3& normal) noexcept;
[[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHQuaternion& q) noexcept; [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHVec3& axis, float angleInRad) noexcept;
[[nodiscard]] static SHVec3 RotateX (const SHVec3& v, float angleInRad) noexcept; [[nodiscard]] static SHVec3 Rotate (const SHVec3& v, const SHQuaternion& q) noexcept;
[[nodiscard]] static SHVec3 RotateY (const SHVec3& v, float angleInRad) noexcept; [[nodiscard]] static SHVec3 RotateX (const SHVec3& v, float angleInRad) noexcept;
[[nodiscard]] static SHVec3 RotateZ (const SHVec3& v, float angleInRad) noexcept; [[nodiscard]] static SHVec3 RotateY (const SHVec3& v, float angleInRad) noexcept;
[[nodiscard]] static SHVec3 Transform (const SHVec3& v, const SHMatrix& transformMtx) 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; 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" #include "SHRigidBody.h"
// Project Headers // Project Headers
#include <complex.h>
#include <numeric> #include <numeric>
#include "Physics/Collision/SHCollider.h" #include "Physics/Collision/SHCollider.h"
@ -664,26 +665,24 @@ namespace SHADE
const float CUSTOM_MASS = 1.0f / invMass; const float CUSTOM_MASS = 1.0f / invMass;
const bool AUTO_MASS = IsAutoMassEnabled(); const bool AUTO_MASS = IsAutoMassEnabled();
const bool INCLUDE_TRIGGERS = IsTriggerInMassData(); const bool INCLUDE_TRIGGERS = IsTriggerInMassData();
const auto& SHAPES = collider->GetCollisionShapes();
// Compute Total mass and store individual masses if custom mass is being used. // Compute Total mass and store individual masses if custom mass is being used.
// Compute local centroid at the same time // Compute local centroid at the same time
// Zero matrix; // Zero matrix;
SHMatrix tmpLocalTensor; SHMatrix J = SHMatrix::Zero;
tmpLocalTensor -= SHMatrix::Identity;
float totalMass = 0.0f; 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) for (auto* shape : SHAPES)
{ {
// We skip triggers by default // We skip triggers by default
if (shape->IsTrigger() && !INCLUDE_TRIGGERS) if (shape->IsTrigger() && !INCLUDE_TRIGGERS)
continue; continue;
shape->ComputeTransforms();
// p = m/v, therefore m = pv. This is the true mass of the shape. // p = m/v, therefore m = pv. This is the true mass of the shape.
const float MASS = shape->GetDensity() * shape->GetVolume(); const float MASS = shape->GetDensity() * shape->GetVolume();
totalMass += MASS; totalMass += MASS;
@ -705,6 +704,7 @@ namespace SHADE
const SHMatrix R = SHMatrix::Rotate(motionState.orientation); const SHMatrix R = SHMatrix::Rotate(motionState.orientation);
const SHMatrix RT = SHMatrix::Transpose(R); 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; worldCentroid = (R * localCentroid) + motionState.position;
for (size_t i = 0; i < SHAPES.size(); ++i) for (size_t i = 0; i < SHAPES.size(); ++i)
@ -721,31 +721,23 @@ namespace SHADE
actualMass *= CUSTOM_MASS / totalMass; actualMass *= CUSTOM_MASS / totalMass;
// Convert inertia tensor into local-space of the body // Convert inertia tensor into local-space of the body
// R * I * RT = R * (I * RT)
SHMatrix I = SHAPE->GetInertiaTensor( actualMass ) * RT; SHMatrix I = SHAPE->GetInertiaTensor( actualMass ) * RT;
I = R * I; I = R * I;
// Parallel Axis Theorem // Parallel Axis Theorem
const SHVec3 O = SHAPE->GetPosition() - worldCentroid; // J = I + m((R /dot R)E_3 - R /outerProduct R)
const float O2 = O.LengthSquared(); const SHVec3 R = SHAPE->GetPosition() - worldCentroid;
const float R_MAG2 = R.LengthSquared();
const SHMatrix R_OX_R = SHVec3::OuterProduct(R, R);
SHMatrix offsetMatrix; J += I + actualMass * (SHMatrix::Identity * R_MAG2 - R_OX_R);
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;
} }
// Set diagonals then invert // Set diagonals then invert
localInvInertia.m[0][0] = tmpLocalTensor.m[0][0]; localInvInertia.m[0][0] = J.m[0][0];
localInvInertia.m[1][1] = tmpLocalTensor.m[1][1]; localInvInertia.m[1][1] = J.m[1][1];
localInvInertia.m[2][2] = tmpLocalTensor.m[2][2]; localInvInertia.m[2][2] = J.m[2][2];
localInvInertia = SHMatrix::Inverse(localInvInertia); localInvInertia = SHMatrix::Inverse(localInvInertia);
} }