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 +468 -167
20 changed files with 468 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 53edffebac - Show all commits

9
SHADE_Engine/src/Editor/EditorWindow/Inspector/SHEditorComponentView.hpp
View File

@ -286,7 +286,14 @@ namespace SHADE
if(ImGui::CollapsingHeader("Debug Information", ImGuiTreeNodeFlags_DefaultOpen))//Dynamic or Kinematic only fields
{
SHEditorWidgets::DragVec3("Position", { "X", "Y", "Z" }, [component] {return component->GetPosition(); }, [](SHVec3 const& value) {}, false, "Position", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
//SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component] {return component->GetRotation(); }, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
SHEditorWidgets::DragVec3("Rotation", { "X", "Y", "Z" }, [component]
{
// Convert it to degrees...
auto rot = component->GetRotation();
for (size_t i = 0; i < SHVec3::SIZE; ++i)
rot[i] = SHMath::RadiansToDegrees(rot[i]);
return rot;
}, [](SHVec3 const& value) {}, false, "Rotation", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);
if (rbType == SHRigidBodyComponent::Type::DYNAMIC || rbType == SHRigidBodyComponent::Type::KINEMATIC) //Dynamic or Kinematic only fields
{
SHEditorWidgets::DragVec3("Velocity", { "X", "Y", "Z" }, [component] {return component->GetLinearVelocity(); }, [](SHVec3 const& value) {}, false, "Linear Velocity", 0.1f, "%.3f", 0.0f, 0.0f, ImGuiSliderFlags_ReadOnly);

12
SHADE_Engine/src/Graphics/MiddleEnd/Interface/SHDebugDrawSystem.cpp
View File

@ -203,9 +203,9 @@ namespace SHADE
drawCube(points, color, pos, size);
}
void SHDebugDrawSystem::DrawSphere(const SHVec4& color, const SHVec3& pos, double radius)
void SHDebugDrawSystem::DrawSphere(const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius)
{
drawSphere(points, color, pos, radius);
drawSphere(points, color, pos, rot, radius);
}
/*---------------------------------------------------------------------------------*/
@ -238,7 +238,7 @@ namespace SHADE
void SHDebugDrawSystem::DrawPersistentSphere(const SHVec4& color, const SHVec3& pos, double radius)
{
drawSphere(persistentPoints, color, pos, radius);
drawSphere(persistentPoints, color, pos, SHVec3::Zero, radius);
}
void SHDebugDrawSystem::ClearPersistentDraws()
@ -315,7 +315,7 @@ namespace SHADE
);
}
void SHDebugDrawSystem::drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, double radius)
void SHDebugDrawSystem::drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius)
{
//if (spherePoints.empty())
{
@ -324,7 +324,9 @@ namespace SHADE
static const SHMeshData SPHERE = SHPrimitiveGenerator::Sphere();
for (const auto& idx : SPHERE.Indices)
{
spherePoints.emplace_back(SPHERE.VertexPositions[idx] * radius + pos);
SHVec3 SCALE { static_cast<float>(radius) };
const SHMatrix TRS = SHMatrix::Transform(pos, rot, { static_cast<float>(radius) });
spherePoints.emplace_back(SHVec3::Transform(SPHERE.VertexPositions[idx], TRS));
}
}
drawLineSet(storage, color, spherePoints.begin(), spherePoints.end());

5
SHADE_Engine/src/Graphics/MiddleEnd/Interface/SHDebugDrawSystem.h
View File

@ -123,8 +123,9 @@ namespace SHADE
/// </summary>
/// <param name="color">Colour of the sphere.</param>
/// <param name="pos">Position where the sphere wil be centered at.</param>
/// <param name="rot">Rotation of the sphere. </param>
/// <param name="size">Size of the rendered sphere.</param>
void DrawSphere(const SHVec4& color, const SHVec3& pos, double radius);
void DrawSphere(const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius);
/*---------------------------------------------------------------------------------*/
/* Persistent Draw Functions */
@ -246,7 +247,7 @@ namespace SHADE
template<typename IterType>
void drawPoly(std::vector<PointVertex>& storage, const SHVec4& color, IterType pointListBegin, IterType pointListEnd);
void drawCube(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& size);
void drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, double radius);
void drawSphere(std::vector<PointVertex>& storage, const SHVec4& color, const SHVec3& pos, const SHVec3& rot, double radius);
};
}

6
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShape.h
View File

@ -111,6 +111,12 @@ namespace SHADE
void SetCollisionTag (SHCollisionTag* newCollisionTag) noexcept;
/*---------------------------------------------------------------------------------*/
/* Member Functions */
/*---------------------------------------------------------------------------------*/
[[nodiscard]] virtual SHMatrix GetInertiaTensor (float mass) const noexcept = 0;
protected:
/*---------------------------------------------------------------------------------*/
/* Data Members */

15
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.cpp
View File

@ -13,6 +13,9 @@
// Primary Header
#include "SHSphereCollisionShape.h"
// Project Headers
#include "Math/SHMatrix.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
@ -179,5 +182,17 @@ namespace SHADE
return SHSphere::Raycast(ray);
}
SHMatrix SHSphereCollisionShape::GetInertiaTensor(float mass) const noexcept
{
static constexpr float TWO_OVER_FIVE = 2.0f / 5.0f;
const float DIAGONAL = TWO_OVER_FIVE * mass * (Radius * Radius);
SHMatrix result;
result.m[0][0] = result.m[1][1] = result.m[2][2] = DIAGONAL;
return result;
}
} // namespace SHADE

16
SHADE_Engine/src/Physics/Collision/CollisionShapes/SHSphereCollisionShape.h
View File

@ -96,7 +96,7 @@ namespace SHADE
* @return
* True if the point is inside the sphere.
*/
bool TestPoint (const SHVec3& point) const noexcept override;
[[nodiscard]] bool TestPoint (const SHVec3& point) const noexcept override;
/**
* @brief
@ -107,9 +107,19 @@ namespace SHADE
* An object holding the results of the raycast. <br/>
* See the corresponding header for the contents of the object.
*/
SHRaycastResult Raycast (const SHRay& ray) const noexcept override;
[[nodiscard]] SHRaycastResult Raycast (const SHRay& ray) const noexcept override;
// TODO: Compute Moment of Inertia
/**
* @brief
* Computes the inertia tensor of the sphere.
* @param mass
* The mass of the sphere.
* @return
* The inertia tensor of the sphere.
*/
[[nodiscard]] SHMatrix GetInertiaTensor (float mass) const noexcept override;
private:
/*---------------------------------------------------------------------------------*/

31
SHADE_Engine/src/Physics/Dynamics/SHMotionState.cpp
View File

@ -80,6 +80,15 @@ namespace SHADE
position = newPosition;
}
void SHMotionState::ForceOrientation(const SHQuaternion& newOrientation) noexcept
{
hasMoved = true;
prevOrientation = newOrientation;
orientation = newOrientation;
}
void SHMotionState::IntegratePosition(const SHVec3& velocity, float dt) noexcept
{
// Velocities are 0 when objects are static or sleeping. We do not want to integrate them here.
@ -91,10 +100,32 @@ namespace SHADE
position += velocity * dt;
}
void SHMotionState::IntegrateOrientation(const SHVec3& velocity, float dt) noexcept
{
// Velocities are 0 when objects are static or sleeping. We do not want to integrate them here.
// This call should never reach here.
hasMoved = true;
prevOrientation = orientation;
SHQuaternion qv{ velocity.x * dt, velocity.y * dt, velocity.z * dt, 0.0f };
qv *= orientation;
orientation += qv * 0.5f;
orientation = SHQuaternion::Normalise(orientation);
}
SHVec3 SHMotionState::InterpolatePositions(float factor) const noexcept
{
return SHVec3::ClampedLerp(prevPosition, position, factor);
}
SHQuaternion SHMotionState::InterpolateOrientations(float factor) const noexcept
{
return SHQuaternion::ClampedSlerp(prevOrientation, orientation, factor);
}
} // namespace SHADE

50
SHADE_Engine/src/Physics/Dynamics/SHMotionState.h
View File

@ -11,8 +11,10 @@
#pragma once
// Project Headers
#include "Math/SHQuaternion.h"
#include "Math/Vector/SHVec3.h"
namespace SHADE
{
/*-------------------------------------------------------------------------------------*/
@ -30,10 +32,13 @@ namespace SHADE
/* Data Members */
/*-----------------------------------------------------------------------------------*/
bool hasMoved;
bool hasMoved;
SHVec3 position;
SHVec3 prevPosition;
SHVec3 position;
SHVec3 prevPosition;
SHQuaternion orientation;
SHQuaternion prevOrientation;
/*-----------------------------------------------------------------------------------*/
/* Constructors & Destructor */
@ -65,17 +70,36 @@ namespace SHADE
* @param newPosition
* The new position to set.
*/
void ForcePosition (const SHVec3& newPosition) noexcept;
void ForcePosition (const SHVec3& newPosition) noexcept;
/**
* @brief
* Integrates the positions using velocity with respect to time.
* Forcefully sets the orientation. Meant to be used when transform overrides the rigid body
* orientations
* @param newOrientation
* The new orientation to set.
*/
void ForceOrientation (const SHQuaternion& newOrientation) noexcept;
/**
* @brief
* Integrates the positions using linear velocity with respect to time.
* @param velocity
* The velocity to integrate.
* The linear velocity to integrate.
* @param dt
* The delta time to integrate with respect to.
*/
void IntegratePosition (const SHVec3& velocity, float dt) noexcept;
void IntegratePosition (const SHVec3& velocity, float dt) noexcept;
/**
* @brief
* Integrates the orientation using angular velocity with respect to time.
* @param velocity
* The angular velocity to integrate.
* @param dt
* The delta time to integrate with respect to.
*/
void IntegrateOrientation (const SHVec3& velocity, float dt) noexcept;
/**
* @brief
@ -85,7 +109,17 @@ namespace SHADE
* @returns
* The interpolated position meant for rendering.
*/
SHVec3 InterpolatePositions (float factor) const noexcept;
SHVec3 InterpolatePositions (float factor) const noexcept;
/**
* @brief
* Interpolates the orientation between the previous and the last using a given factor.
* @param factor
* The factor to interpolate by. Should be between 0 & 1.
* @returns
* The interpolated orientation meant for rendering.
*/
SHQuaternion InterpolateOrientations (float factor) const noexcept;
};

23
SHADE_Engine/src/Physics/Dynamics/SHPhysicsWorld.cpp
View File

@ -74,7 +74,10 @@ namespace SHADE
void SHPhysicsWorld::Step(float dt)
{
for (auto* rigidBody : rigidBodies)
{
rigidBody->ComputeWorldData();
integrateForces(*rigidBody, dt);
}
for (auto* rigidBody : rigidBodies)
integrateVelocities(*rigidBody, dt);
@ -93,10 +96,14 @@ namespace SHADE
const SHVec3 LINEAR_ACCELERATION = rigidBody.accumulatedForce * rigidBody.invMass;
const SHVec3 GRAVITATIONAL_ACCELERATION = rigidBody.IsGravityEnabled() ? settings.gravity * rigidBody.gravityScale : SHVec3::Zero;
rigidBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
rigidBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * dt;
// Integrate torque into angular velocity
rigidBody.angularVelocity += rigidBody.worldInvInertia * (rigidBody.accumulatedTorque * dt);
// Apply drag (exponentially applied)
rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
rigidBody.linearVelocity *= 1.0f / (1.0f + dt * rigidBody.linearDrag);
rigidBody.angularVelocity *= 1.0f / (1.0f + dt * rigidBody.angularDrag);
}
void SHPhysicsWorld::integrateVelocities(SHRigidBody& rigidBody, float dt) const noexcept
@ -111,7 +118,13 @@ namespace SHADE
, rigidBody.GetFreezePositionZ() ? 0.0f : rigidBody.linearVelocity.z
};
// TODO: Enforce angular constraints
// Enforce angular constraints
rigidBody.angularVelocity = SHVec3
{
rigidBody.GetFreezeRotationX() ? 0.0f : rigidBody.angularVelocity.x
, rigidBody.GetFreezeRotationY() ? 0.0f : rigidBody.angularVelocity.y
, rigidBody.GetFreezeRotationZ() ? 0.0f : rigidBody.angularVelocity.z
};
};
// Always reset movement flag
@ -129,13 +142,13 @@ namespace SHADE
ENFORCE_CONSTRAINED_VELOCITIES(rigidBody);
rigidBody.motionState.IntegratePosition(rigidBody.linearVelocity, dt);
// TODO: Integrate orientations
rigidBody.motionState.IntegrateOrientation(rigidBody.angularVelocity, dt);
// Sync with collider transforms if a collider is present
if (rigidBody.collider)
{
rigidBody.collider->SetPosition(rigidBody.motionState.position);
// TODO: Sync orientations
rigidBody.collider->SetOrientation(rigidBody.motionState.orientation);
rigidBody.collider->RecomputeShapes();
}

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

@ -27,9 +27,10 @@ namespace SHADE
: entityID { eid }
, collider { nullptr }
, bodyType { type }
, gravityScale { 1.0f }
, invMass { type == Type::DYNAMIC ? 1.0f : 0.0f }
, linearDrag { 0.01f }
, gravityScale { 1.0f }
, angularDrag { 0.01f }
, flags { 0U }
{
// Set default flags
@ -41,14 +42,19 @@ namespace SHADE
}
SHRigidBody::SHRigidBody(const SHRigidBody& rhs) noexcept
: entityID { rhs.entityID }
, collider { nullptr }
, bodyType { rhs.bodyType }
, invMass { rhs.invMass }
, linearDrag { rhs.linearDrag }
, gravityScale { rhs.gravityScale }
, flags { rhs.flags }
, motionState { rhs.motionState }
: entityID { rhs.entityID }
, collider { nullptr }
, bodyType { rhs.bodyType }
, gravityScale { rhs.gravityScale }
, invMass { rhs.invMass }
, linearDrag { rhs.linearDrag }
, angularDrag { rhs.angularDrag }
, localInvInertia { rhs.localInvInertia }
, worldInvInertia { rhs.worldInvInertia }
, localCentroid { rhs.localCentroid }
, worldCentroid { rhs.worldCentroid }
, flags { rhs.flags }
, motionState { rhs.motionState }
{
// All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
}
@ -57,9 +63,14 @@ namespace SHADE
: entityID { rhs.entityID }
, collider { nullptr }
, bodyType { rhs.bodyType }
, gravityScale { rhs.gravityScale }
, invMass { rhs.invMass }
, linearDrag { rhs.linearDrag }
, gravityScale { rhs.gravityScale }
, angularDrag { rhs.angularDrag }
, localInvInertia { rhs.localInvInertia }
, worldInvInertia { rhs.worldInvInertia }
, localCentroid { rhs.localCentroid }
, worldCentroid { rhs.worldCentroid }
, flags { rhs.flags }
, motionState { std::move(rhs.motionState) }
{
@ -77,17 +88,24 @@ namespace SHADE
return *this;
entityID = rhs.entityID;
// Deep copy the collider
*collider = *rhs.collider;
bodyType = rhs.bodyType;
invMass = rhs.invMass;
linearDrag = rhs.linearDrag;
gravityScale = rhs.gravityScale;
flags = rhs.flags;
motionState = rhs.motionState;
*collider = *rhs.collider;
bodyType = rhs.bodyType;
gravityScale = rhs.gravityScale;
invMass = rhs.invMass;
linearDrag = rhs.linearDrag;
angularDrag = rhs.angularDrag;
localInvInertia = rhs.localInvInertia;
worldInvInertia = rhs.worldInvInertia;
localCentroid = rhs.localCentroid;
worldCentroid = rhs.worldCentroid;
flags = rhs.flags;
motionState = rhs.motionState;
// All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
accumulatedForce = SHVec3::Zero;
accumulatedTorque = SHVec3::Zero;
linearVelocity = SHVec3::Zero;
angularVelocity = SHVec3::Zero;
@ -97,17 +115,24 @@ namespace SHADE
SHRigidBody& SHRigidBody::operator=(SHRigidBody&& rhs) noexcept
{
entityID = rhs.entityID;
// Deep copy the collider
*collider = *rhs.collider;
bodyType = rhs.bodyType;
gravityScale = rhs.gravityScale;
invMass = rhs.invMass;
linearDrag = rhs.linearDrag;
gravityScale = rhs.gravityScale;
angularDrag = rhs.angularDrag;
localInvInertia = rhs.localInvInertia;
worldInvInertia = rhs.worldInvInertia;
localCentroid = rhs.localCentroid;
worldCentroid = rhs.worldCentroid;
flags = rhs.flags;
motionState = std::move(rhs.motionState);
// All other properties are defaulted to 0 to not carry over any potential errors / invalid values.
accumulatedForce = SHVec3::Zero;
accumulatedTorque = SHVec3::Zero;
linearVelocity = SHVec3::Zero;
angularVelocity = SHVec3::Zero;
@ -123,6 +148,11 @@ namespace SHADE
return bodyType;
}
float SHRigidBody::GetGravityScale() const noexcept
{
return gravityScale;
}
float SHRigidBody::GetMass() const noexcept
{
return 1.0f/ invMass;
@ -133,16 +163,41 @@ namespace SHADE
return linearDrag;
}
float SHRigidBody::GetGravityScale() const noexcept
float SHRigidBody::GetAngularDrag() const noexcept
{
return gravityScale;
return angularDrag;
}
const SHVec3& SHRigidBody::GetAccumulatedForce() const noexcept
const SHMatrix& SHRigidBody::GetLocalInvInertia() const noexcept
{
return localInvInertia;
}
const SHMatrix& SHRigidBody::GetWorldInvInertia() const noexcept
{
return worldInvInertia;
}
const SHVec3& SHRigidBody::GetLocalCentroid() const noexcept
{
return localCentroid;
}
const SHVec3& SHRigidBody::GetWorldCentroid() const noexcept
{
return worldCentroid;
}
const SHVec3& SHRigidBody::GetForce() const noexcept
{
return accumulatedForce;
}
const SHVec3& SHRigidBody::GetTorque() const noexcept
{
return accumulatedTorque;
}
const SHVec3& SHRigidBody::GetLinearVelocity() const noexcept
{
return linearVelocity;
@ -226,7 +281,6 @@ namespace SHADE
return motionState;
}
/*-----------------------------------------------------------------------------------*/
/* Setter Functions Definitions */
/*-----------------------------------------------------------------------------------*/
@ -245,6 +299,11 @@ namespace SHADE
invMass = newType == Type::DYNAMIC ? 1.0f : 0.0f;
}
void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
{
gravityScale = newGravityScale;
}
void SHRigidBody::SetMass(float newMass) noexcept
{
if (bodyType != Type::DYNAMIC)
@ -261,7 +320,7 @@ namespace SHADE
invMass = 1.0f / newMass;
// TODO: Recompute inertia tensor
ComputeMassData();
}
void SHRigidBody::SetLinearDrag(float newLinearDrag) noexcept
@ -281,16 +340,45 @@ namespace SHADE
linearDrag = newLinearDrag;
}
void SHRigidBody::SetGravityScale(float newGravityScale) noexcept
void SHRigidBody::SetAngularDrag(float newAngularDrag) noexcept
{
gravityScale = newGravityScale;
if (bodyType == Type::STATIC)
{
SHLOG_WARNING("Cannot set angular drag of a Static Body {}", entityID)
return;
}
if (newAngularDrag < 0.0f)
{
SHLOG_WARNING("Cannot set drag below 0. Object {}'s angular drag will remain unchanged.", entityID)
return;
}
angularDrag = newAngularDrag;
}
void SHRigidBody::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
{
if (bodyType == Type::STATIC)
{
SHLOG_WARNING("Cannot set linear velocity of a Static Body {}", entityID)
return;
}
linearVelocity = newLinearVelocity;
}
void SHRigidBody::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
{
if (bodyType == Type::STATIC)
{
SHLOG_WARNING("Cannot set angular velocity of a Static Body {}", entityID)
return;
}
angularVelocity = newAngularVelocity;
}
void SHRigidBody::SetIsActive(bool isActive) noexcept
{
static constexpr unsigned int FLAG_POS = 0;
@ -304,7 +392,18 @@ namespace SHADE
static constexpr unsigned int FLAG_POS = 1;
static constexpr uint16_t VALUE = 1U << FLAG_POS;
isSleeping ? flags |= VALUE : flags &= ~VALUE;
if (isSleeping)
{
flags |= VALUE;
ClearForces();
linearVelocity = SHVec3::Zero;
angularVelocity = SHVec3::Zero;
}
else
{
flags &= ~VALUE;
}
}
void SHRigidBody::SetSleepingEnabled(bool enableSleeping) noexcept
@ -411,6 +510,8 @@ namespace SHADE
flags |= VALUE;
// Reset angular velocity along X-axis
angularVelocity.x = 0.0f;
// Set inertia tensor on the x-axis to 0
localInvInertia.m[0][0] = worldInvInertia.m[0][0] = 0.0f;
}
else
{
@ -428,6 +529,8 @@ namespace SHADE
flags |= VALUE;
// Reset angular velocity along Y-axis
angularVelocity.y = 0.0f;
// Set inertia tensor on the y-axis to 0
localInvInertia.m[1][1] = worldInvInertia.m[1][1] = 0.0f;
}
else
{
@ -445,6 +548,8 @@ namespace SHADE
flags |= VALUE;
// Reset angular velocity along Z-axis
angularVelocity.z = 0.0f;
// Set inertia tensor on the z-axis to 0
localInvInertia.m[2][2] = worldInvInertia.m[2][2] = 0.0f;
}
else
{
@ -461,21 +566,60 @@ namespace SHADE
if (bodyType != Type::DYNAMIC)
return;
accumulatedForce += force;
// Compute torque when force is offset
accumulatedForce += force;
accumulatedTorque += SHVec3::Cross(pos, force);
}
void SHRigidBody::AddImpulse(const SHVec3& impulse, const SHVec3& pos) noexcept
{
if (bodyType != Type::DYNAMIC)
return;
return;
linearVelocity += impulse * invMass;
linearVelocity += impulse * invMass;
angularVelocity += worldInvInertia * SHVec3::Cross(pos, impulse);
}
void SHRigidBody::AddTorque(const SHVec3& torque) noexcept
{
if (bodyType != Type::DYNAMIC)
return;
accumulatedTorque += torque;
}
void SHRigidBody::ClearForces() noexcept
{
accumulatedForce = SHVec3::Zero;
accumulatedForce = SHVec3::Zero;
accumulatedTorque = SHVec3::Zero;
}
void SHRigidBody::ComputeWorldData() noexcept
{
const SHMatrix ROTATION = SHMatrix::Rotate(motionState.orientation);
// Compute world inertia
worldInvInertia = ROTATION * localInvInertia * SHMatrix::Transpose(ROTATION);
// Compute world centroid
worldCentroid = (ROTATION * localCentroid) + motionState.position;
}
void SHRigidBody::ComputeMassData() noexcept
{
// TODO: Compute total inertia and centroid from composited colliders using the Parallel Axis Theorem.
// TODO: If auto mass in enabled, compute total mass based from each collider.
// TODO: If auto mass disabled, compute inertia tensor for each shape using the ratio of its mass / total mass by comparing the volume / total volume.
if (collider && !collider->GetCollisionShapes().empty())
{
// HACK: For now, take only the first shape as we are testing with non-composited colliders. We are using the center as the centroid.
const auto* FIRST_SHAPE = collider->GetCollisionShape(0);
localInvInertia = SHMatrix::Inverse(FIRST_SHAPE->GetInertiaTensor(1.0f / invMass));
}
else
{
localInvInertia.m[0][0] = localInvInertia.m[1][1] = localInvInertia.m[2][2] = invMass;
}
}
} // namespace SHADE

74
SHADE_Engine/src/Physics/Dynamics/SHRigidBody.h
View File

@ -12,6 +12,7 @@
// Project Headers
#include "ECS_Base/Entity/SHEntity.h"
#include "Math/SHMatrix.h"
#include "Math/Vector/SHVec3.h"
#include "SHMotionState.h"
@ -73,12 +74,20 @@ namespace SHADE
[[nodiscard]] Type GetType () const noexcept;
[[nodiscard]] float GetMass () const noexcept;
[[nodiscard]] float GetLinearDrag () const noexcept;
[[nodiscard]] float GetGravityScale () const noexcept;
[[nodiscard]] const SHVec3& GetAccumulatedForce () 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& 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;
@ -111,6 +120,8 @@ namespace SHADE
*/
void SetType (Type newType) noexcept;
void SetGravityScale (float newGravityScale) noexcept;
/**
* @brief
* Mass is only modifiable for Dynamic bodies.
@ -127,9 +138,16 @@ namespace SHADE
*/
void SetLinearDrag (float newLinearDrag) noexcept;
/**
* @brief
* Drag is only modifiable for non-Static bodies.
* @param newAngularDrag
* The new drag to set. Values below 0 will be ignored.
*/
void SetAngularDrag (float newAngularDrag) noexcept;
void SetGravityScale (float newGravityScale) noexcept;
void SetLinearVelocity (const SHVec3& newLinearVelocity) noexcept;
void SetAngularVelocity (const SHVec3& newAngularVelocity)noexcept;
// Flags
@ -151,29 +169,55 @@ namespace SHADE
/**
* @brief
* Adds a force to the body with an offset from it's center of mass.
* Adds a force to the body with an offset from it's center of mass. <br/>
* Non-dynamic bodies will be ignored.
* @param force
* The force to add to the body.
* @param pos
* The position from the center of mass to offset the force. Defaults to zero.
*/
void AddForce (const SHVec3& force, const SHVec3& pos = SHVec3::Zero) noexcept;
void AddForce (const SHVec3& force, const SHVec3& pos = SHVec3::Zero) noexcept;
/**
* @brief
* Adds an impulse to the body with an offset from it's center of mass.
* Adds an impulse to the body with an offset from it's center of mass. <br/>
* Non-dynamic bodies will be ignored.
* @param impulse
* The impulse to add to the body.
* @param pos
* The position from the center of mass to offset the impulse. Defaults to zero.
*/
void AddImpulse (const SHVec3& impulse, const SHVec3& pos = SHVec3::Zero) noexcept;
void AddImpulse (const SHVec3& impulse, const SHVec3& pos = SHVec3::Zero) noexcept;
/**
* @brief
* Adds torque to rotate the body about it's centroid. <br/>
* Non-dynamic bodies will be ignored.
* @param torque
* The torque to add to the body.
*/
void AddTorque (const SHVec3& torque) noexcept;
/**
* @brief
* Removes all the forces from the body.
*/
void ClearForces () noexcept;
void ClearForces () noexcept;
/**
* @brief
* Computes the centroid and invInertia in world space.
*/
void ComputeWorldData () noexcept;
/**
* @brief
* Computes the centroid and inertia of the object. <br/>
* If auto-mass is enabled, computes the mass. <br/>
* If auto-mass is disabled, the inertia is computed based on the ratio each shape's volume over the total volume.
*
*/
void ComputeMassData () noexcept;
private:
/*-----------------------------------------------------------------------------------*/
@ -186,12 +230,20 @@ namespace SHADE
Type bodyType;
float gravityScale;
float invMass;
float linearDrag;
float angularDrag;
float gravityScale;
SHMatrix localInvInertia;
SHMatrix worldInvInertia;
SHVec3 localCentroid;
SHVec3 worldCentroid;
SHVec3 accumulatedForce;
SHVec3 accumulatedTorque;
SHVec3 linearVelocity;
SHVec3 angularVelocity;

144
SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.cpp
View File

@ -36,18 +36,12 @@ namespace SHADE
bool SHRigidBodyComponent::IsGravityEnabled() const noexcept
{
if (rigidBody)
return rigidBody->IsGravityEnabled();
return false;
return rigidBody ? rigidBody->IsGravityEnabled() : false;
}
bool SHRigidBodyComponent::IsAllowedToSleep() const noexcept
{
if (rigidBody)
return rigidBody->IsSleepingEnabled();
return false;
return rigidBody ? rigidBody->IsSleepingEnabled() : false;
}
bool SHRigidBodyComponent::IsInterpolating() const noexcept
@ -57,131 +51,93 @@ namespace SHADE
bool SHRigidBodyComponent::IsSleeping() const noexcept
{
if (rigidBody)
return rigidBody->IsSleeping();
return false;
return rigidBody ? rigidBody->IsSleeping() : false;
}
bool SHRigidBodyComponent::GetAutoMass() const noexcept
{
if (rigidBody)
return rigidBody->IsAutoMassEnabled();
return false;
return rigidBody ? rigidBody->IsAutoMassEnabled() : false;
}
bool SHRigidBodyComponent::GetFreezePositionX() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezePositionX();
return false;
return rigidBody ? rigidBody->GetFreezePositionX() : false;
}
bool SHRigidBodyComponent::GetFreezePositionY() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezePositionY();
return false;
return rigidBody ? rigidBody->GetFreezePositionY() : false;
}
bool SHRigidBodyComponent::GetFreezePositionZ() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezePositionZ();
return false;
return rigidBody ? rigidBody->GetFreezePositionZ() : false;
}
bool SHRigidBodyComponent::GetFreezeRotationX() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezeRotationX();
return false;
return rigidBody ? rigidBody->GetFreezeRotationX() : false;
}
bool SHRigidBodyComponent::GetFreezeRotationY() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezeRotationY();
return false;
return rigidBody ? rigidBody->GetFreezeRotationY() : false;
}
bool SHRigidBodyComponent::GetFreezeRotationZ() const noexcept
{
if (rigidBody)
return rigidBody->GetFreezeRotationZ();
return false;
return rigidBody ? rigidBody->GetFreezeRotationZ() : false;
}
float SHRigidBodyComponent::GetGravityScale() const noexcept
{
if (rigidBody)
return rigidBody->GetGravityScale();
return 0.0f;
return rigidBody ? rigidBody->GetGravityScale() : 0.0f;
}
float SHRigidBodyComponent::GetMass() const noexcept
{
if (rigidBody)
return rigidBody->GetMass();
return -1.0f;
return rigidBody ? rigidBody->GetMass() : -1.0f;
}
float SHRigidBodyComponent::GetDrag() const noexcept
{
if (rigidBody)
return rigidBody->GetLinearDrag();
return -1.0f;
return rigidBody ? rigidBody->GetLinearDrag() : -1.0f;
}
float SHRigidBodyComponent::GetAngularDrag() const noexcept
{
return 0.0f;
return rigidBody ? rigidBody->GetAngularDrag() : -1.0f;
}
SHVec3 SHRigidBodyComponent::GetForce() const noexcept
{
if (rigidBody)
return rigidBody->GetAccumulatedForce();
return SHVec3::Zero;
return rigidBody ? rigidBody->GetForce() : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetTorque() const noexcept
{
return SHVec3::Zero;
return rigidBody ? rigidBody->GetTorque() : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetLinearVelocity() const noexcept
{
if (rigidBody)
return rigidBody->GetLinearVelocity();
return SHVec3::Zero;
return rigidBody ? rigidBody->GetLinearVelocity() : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetAngularVelocity() const noexcept
{
return SHVec3::Zero;
return rigidBody ? rigidBody->GetAngularVelocity() : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetPosition() const noexcept
{
if (rigidBody)
return rigidBody->GetMotionState().position;
return SHVec3::Zero;
return rigidBody ? rigidBody->GetMotionState().position : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetRotation() const noexcept
{
return rigidBody ? rigidBody->GetMotionState().orientation.ToEuler() : SHVec3::Zero;
}
/*-----------------------------------------------------------------------------------*/
/* Setter Functions Definitions */
@ -282,7 +238,8 @@ namespace SHADE
void SHRigidBodyComponent::SetAngularDrag(float newAngularDrag) noexcept
{
if (rigidBody)
rigidBody->SetAngularDrag(newAngularDrag);
}
void SHRigidBodyComponent::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
@ -293,7 +250,8 @@ namespace SHADE
void SHRigidBodyComponent::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
{
if (rigidBody)
rigidBody->SetAngularVelocity(newAngularVelocity);
}
/*-----------------------------------------------------------------------------------*/
@ -308,38 +266,67 @@ namespace SHADE
void SHRigidBodyComponent::AddForceAtLocalPos(const SHVec3& force, const SHVec3& localPos) const noexcept
{
if (rigidBody)
rigidBody->AddForce(force, localPos);
}
void SHRigidBodyComponent::AddForceAtWorldPos(const SHVec3& force, const SHVec3& worldPos) const noexcept
{
if (rigidBody)
rigidBody->AddForce(force, worldPos);
{
// Convert world pos into local space of the body
const SHVec3 LOCAL_POS = worldPos - rigidBody->GetWorldCentroid();
rigidBody->AddForce(force, LOCAL_POS);
}
}
void SHRigidBodyComponent::AddRelativeForce(const SHVec3& relativeForce) const noexcept
{
if (rigidBody)
{
// Rotate force into world space
const SHVec3 FORCE = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
rigidBody->AddForce(FORCE);
}
}
void SHRigidBodyComponent::AddRelativeForceAtLocalPos(const SHVec3& relativeForce, const SHVec3& localPos) const noexcept
{
if (rigidBody)
{
// Rotate force into world space
const SHVec3 FORCE = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
rigidBody->AddForce(FORCE, localPos);
}
}
void SHRigidBodyComponent::AddRelativeForceAtWorldPos(const SHVec3& relativeForce, const SHVec3& worldPos) const noexcept
{
if (rigidBody)
{
// Rotate force into world space
const SHVec3 FORCE = SHVec3::Rotate(relativeForce, rigidBody->GetMotionState().orientation);
// Convert world pos into local space of the body
const SHVec3 LOCAL_POS = worldPos - rigidBody->GetWorldCentroid();
rigidBody->AddForce(FORCE, LOCAL_POS);
}
}
void SHRigidBodyComponent::AddTorque(const SHVec3& torque) const noexcept
{
if (rigidBody)
rigidBody->AddTorque(torque);
}
void SHRigidBodyComponent::AddRelativeTorque(const SHVec3& relativeTorque) const noexcept
{
if (rigidBody)
{
// Rotate force into world space
const SHVec3 TORQUE = SHVec3::Rotate(relativeTorque, rigidBody->GetMotionState().orientation);
rigidBody->AddTorque(TORQUE);
}
}
void SHRigidBodyComponent::ClearForces() const noexcept
@ -348,11 +335,6 @@ namespace SHADE
rigidBody->ClearForces();
}
void SHRigidBodyComponent::ClearTorque() const noexcept
{
}
/*-----------------------------------------------------------------------------------*/
/* Public Function Member Definitions */
/*-----------------------------------------------------------------------------------*/

2
SHADE_Engine/src/Physics/Interface/SHRigidBodyComponent.h
View File

@ -93,6 +93,7 @@ namespace SHADE
[[nodiscard]] SHVec3 GetAngularVelocity () const noexcept;
[[nodiscard]] SHVec3 GetPosition () const noexcept;
[[nodiscard]] SHVec3 GetRotation () const noexcept;
/*---------------------------------------------------------------------------------*/
/* Setter Functions */
@ -138,7 +139,6 @@ namespace SHADE
void AddRelativeTorque (const SHVec3& relativeTorque) const noexcept;
void ClearForces () const noexcept;
void ClearTorque () const noexcept;
private:
/*---------------------------------------------------------------------------------*/

18
SHADE_Engine/src/Physics/System/Routines/SHPhysicsPostUpdateRoutine.cpp
View File

@ -62,15 +62,23 @@ namespace SHADE
if (!MOTION_STATE)
continue;
SHVec3 renderPosition = rigidBodyComponent.IsInterpolating() ? MOTION_STATE.InterpolatePositions(FACTOR) : MOTION_STATE.position;
if (rigidBodyComponent.IsInterpolating())
{
const SHVec3 RENDER_POSITION = MOTION_STATE.InterpolatePositions(FACTOR);
const SHQuaternion RENDER_ORIENTATION = MOTION_STATE.InterpolateOrientations(FACTOR);
transformComponent->SetWorldPosition(RENDER_POSITION);
transformComponent->SetWorldOrientation(RENDER_ORIENTATION);
}
else
{
transformComponent->SetWorldPosition(MOTION_STATE.position);
transformComponent->SetWorldOrientation(MOTION_STATE.orientation);
}
/*
* TODO: Test if the scene graph transforms abides by setting world position. Collisions will ignore the scene graph hierarchy.
*/
transformComponent->SetWorldPosition(renderPosition);
// TODO: SetOrientation
}
// Collision & Trigger messages

2
SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp
View File

@ -54,7 +54,7 @@ namespace SHADE
SHMotionState& motionState = physicsObject.rigidBody->GetMotionState();
motionState.ForcePosition(TRANSFORM_COMPONENT->GetWorldPosition());
// TODO: Force Orientation
motionState.ForceOrientation(TRANSFORM_COMPONENT->GetWorldOrientation());
}
if (physicsObject.collider)

9
SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp
View File

@ -120,7 +120,10 @@ namespace SHADE
case SHCollisionShape::Type::SPHERE:
{
const SHSphereCollisionShape* SPHERE = dynamic_cast<const SHSphereCollisionShape*>(SHAPE);
drawSphere(debugDrawSystem, *SPHERE);
// Compute rotation of sphere
const SHVec3 ROTATION = collider.GetOrientation().ToEuler() + SPHERE->GetRotationOffset();
drawSphere(debugDrawSystem, *SPHERE, ROTATION);
break;
}
@ -137,10 +140,10 @@ namespace SHADE
}
}
void SHPhysicsDebugDrawSystem::drawSphere(SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere) noexcept
void SHPhysicsDebugDrawSystem::drawSphere(SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation) noexcept
{
const SHColour& DRAW_COLOUR = DEBUG_DRAW_COLOURS[SHUtilities::ConvertEnum(sphere.IsTrigger() ? Colours::TRIGGER : Colours::COLLIDER)];
debugDrawSystem->DrawSphere(DRAW_COLOUR, sphere.GetCenter(), sphere.GetWorldRadius());
debugDrawSystem->DrawSphere(DRAW_COLOUR, sphere.GetCenter(), rotation, sphere.GetWorldRadius());
}
void SHPhysicsDebugDrawSystem::drawContact(SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept

4
SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h
View File

@ -138,8 +138,8 @@ namespace SHADE
SHEventHandle onColliderDraw(SHEventPtr onColliderDrawEvent);
static void drawCollider (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider) noexcept;
static void drawSphere (SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere) noexcept;
static void drawCollider (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider) noexcept;
static void drawSphere (SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation) noexcept;
static void drawContact (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept;
static void drawRaycast (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Raycast& raycastInfo) noexcept;

2
SHADE_Engine/src/Tools/SHDebugDraw.cpp
View File

@ -68,7 +68,7 @@ namespace SHADE
void SHDebugDraw::Sphere(const SHVec4& color, const SHVec3& pos, double radius)
{
dbgDrawSys->DrawSphere(color, pos, radius);
dbgDrawSys->DrawSphere(color, pos, SHVec3::Zero, radius);
}
void SHDebugDraw::PersistentLine(const SHVec4& color, const SHVec3& startPt, const SHVec3& endPt)

6
SHADE_Managed/src/Components/RigidBody.cxx
View File

@ -217,10 +217,4 @@ namespace SHADE
{
return Convert::ToCLI(GetNativeComponent()->GetTorque());
}
void RigidBody::ClearTorque()
{
GetNativeComponent()->ClearTorque();
}
}

1
SHADE_Managed/src/Components/RigidBody.hxx
View File

@ -155,7 +155,6 @@ namespace SHADE
void AddRelativeTorque(Vector3 relativeForce);
Vector3 GetTorque();
void ClearTorque();
};
}