Reworked Physics Simulate Body #429

Merged
direnbharwani merged 4 commits from SP3-2-Physics into main 2023-03-20 14:56:29 +08:00
7 changed files with 230 additions and 94 deletions

View File

@ -0,0 +1,51 @@
/****************************************************************************************
* \file SHGhostBody.cpp
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Implementation for the Ghost Body meant for Simulation.
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#include <SHpch.h>
// Primary Header
#include "SHGhostBody.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Constructors & Destructor Definitions */
/*-----------------------------------------------------------------------------------*/
SHGhostBody::SHGhostBody(const SHRigidBodyComponent& rigidBody) noexcept
: linearVelocity { rigidBody.GetLinearVelocity() }
, angularVelocity { rigidBody.GetAngularVelocity() }
, localCentroid { rigidBody.GetLocalCentroid() }
, accumulatedForce { rigidBody.GetForce() }
, accumulatedTorque{ rigidBody.GetTorque() }
, gravityScale { rigidBody.GetGravityScale() }
, mass { rigidBody.GetMass() }
, drag { rigidBody.GetDrag() }
, angularDrag { rigidBody.GetAngularDrag() }
, position { rigidBody.GetPosition() }
, orientation { SHQuaternion::FromEuler(rigidBody.GetRotation()) }
, useGravity { rigidBody.IsGravityEnabled() }
{
const SHVec3 LOCAL_INERTIA = rigidBody.GetLocalInertia();
localInvInertia.x = 1.0f / LOCAL_INERTIA.x;
localInvInertia.y = 1.0f / LOCAL_INERTIA.y;
localInvInertia.z = 1.0f / LOCAL_INERTIA.z;
linearLock.x = rigidBody.GetFreezePositionX() ? 1.0f : 0.0f;
linearLock.y = rigidBody.GetFreezePositionY() ? 1.0f : 0.0f;
linearLock.z = rigidBody.GetFreezePositionZ() ? 1.0f : 0.0f;
angularLock.x = rigidBody.GetFreezeRotationX() ? 1.0f : 0.0f;
angularLock.y = rigidBody.GetFreezeRotationY() ? 1.0f : 0.0f;
angularLock.z = rigidBody.GetFreezeRotationZ() ? 1.0f : 0.0f;
}
} // namespace SHADE

View File

@ -0,0 +1,64 @@
/****************************************************************************************
* \file SHGhostBody.h
* \author Diren D Bharwani, diren.dbharwani, 390002520
* \brief Interface for the Ghost Body meant for Simulation.
*
* \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
* disclosure of this file or its contents without the prior written consent
* of DigiPen Institute of Technology is prohibited.
****************************************************************************************/
#pragma once
// Project Headers
#include "Physics/Interface/SHRigidBodyComponent.h"
namespace SHADE
{
/*-----------------------------------------------------------------------------------*/
/* Type Definitions */
/*-----------------------------------------------------------------------------------*/
/**
* @brief
* Encapsulates a rigid body that will be simulated in the world, but doesn't actually
* exist in the world.
*/
struct SHGhostBody
{
public:
/*---------------------------------------------------------------------------------*/
/* Data Members */
/*---------------------------------------------------------------------------------*/
SHVec3 linearVelocity = SHVec3::Zero;
SHVec3 angularVelocity = SHVec3::Zero;
SHVec3 localInvInertia = SHVec3::One;
SHVec3 localCentroid = SHVec3::Zero;
SHVec3 accumulatedForce = SHVec3::Zero;
SHVec3 accumulatedTorque = SHVec3::Zero;
SHVec3 linearLock = SHVec3::One;
SHVec3 angularLock = SHVec3::One;
float gravityScale = 1.0f;
float mass = 1.0f;
float drag = 0.01f;
float angularDrag = 0.01f;
SHVec3 position = SHVec3::Zero;
SHQuaternion orientation = SHQuaternion::Identity;
bool useGravity = true;
/*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */
/*---------------------------------------------------------------------------------*/
SHGhostBody () noexcept = default;
SHGhostBody (const SHRigidBodyComponent& rigidBody) noexcept;
};
} // namespace SHADE

View File

@ -1,5 +1,7 @@
#include "SHpch.h"
#include "SHTrajectoryRenderingSubSystem.h"
#include "../../../../SHGhostBody.h"
#include "ECS_Base/Managers/SHComponentManager.h"
#include "Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderableComponent.h"
#include "Graphics/Devices/SHVkLogicalDevice.h"
@ -85,17 +87,25 @@ namespace SHADE
{
std::vector<SHVec3> positions{};
std::vector<SHQuaternion> quats{};
physicsSystem->SimulateBody
(positions, quats,
SHPhysicsSystem::SimulateBodyInfo
SHGhostBody defaultGhostBody{};
SHPhysicsSystem::SimulateBodyInfo simulateInfo
{
.bodyEID = entityToSimulate,
.force = comp.GetSimulationForce(),
.continuousForce = false,
.timeStep = comp.GetSimulationTimestep(),
.maxSteps = static_cast<int>(comp.GetSimulationMaxSteps()),
}
);
};
SHPhysicsSystem::SimulateBodyOutput output
{
.positions = &positions
, .orientations = &quats
};
physicsSystem->SimulateBody(defaultGhostBody, simulateInfo, output);
comp.ResetSimulationInfo();

View File

@ -150,6 +150,16 @@ namespace SHADE
return rigidBody ? SHQuaternion{ rigidBody->getTransform().getOrientation() }.ToEuler() : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetLocalInertia() const noexcept
{
return rigidBody ? SHVec3{ rigidBody->getLocalInertiaTensor() } : SHVec3::Zero;
}
SHVec3 SHRigidBodyComponent::GetLocalCentroid() const noexcept
{
return rigidBody ? SHVec3{ rigidBody->getLocalCenterOfMass() } : SHVec3::Zero;
}
/*-----------------------------------------------------------------------------------*/
/* Setter Function Definitions */
/*-----------------------------------------------------------------------------------*/

View File

@ -103,6 +103,9 @@ namespace SHADE
[[nodiscard]] SHVec3 GetPosition () const noexcept;
[[nodiscard]] SHVec3 GetRotation () const noexcept;
[[nodiscard]] SHVec3 GetLocalInertia () const noexcept;
[[nodiscard]] SHVec3 GetLocalCentroid () const noexcept;
/*---------------------------------------------------------------------------------*/
/* Setter Functions */
/*---------------------------------------------------------------------------------*/

View File

@ -14,6 +14,7 @@
#include "SHPhysicsSystem.h"
// Project Headers
#include "../../../SHGhostBody.h"
#include "Assets/SHAssetMacros.h"
#include "ECS_Base/Managers/SHComponentManager.h"
#include "ECS_Base/Managers/SHEntityManager.h"
@ -236,46 +237,22 @@ namespace SHADE
return IS_COLLIDING;
}
void SHPhysicsSystem::SimulateBody(std::vector<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo)
void SHPhysicsSystem::SimulateBody(SHGhostBody& ghostBody, SimulateBodyInfo& simInfo, SimulateBodyOutput& output)
{
// Check for a valid rigidbody
const auto* rigidBody = SHComponentManager::GetComponent_s<SHRigidBodyComponent>(simInfo.bodyEID);
if (!rigidBody)
{
SHLOG_WARNING("Entity {} does not have a rigid body to simulate!", simInfo.bodyEID)
return;
}
// Ignore non-dynamic bodies
if (rigidBody->type != SHRigidBodyComponent::Type::DYNAMIC)
{
SHLOG_WARNING("Entity {} is not a dynamic body. We cannot simulate non-dynamic bodies!", simInfo.bodyEID)
return;
SHLOG_WARNING("Entity {} does not have a rigid body to simulate! This body will collide with everything!", simInfo.bodyEID)
}
// Prepare simulation info (I'm basically declaring an entire body here)
SHVec3 bodyPosition = rigidBody->GetPosition();
SHQuaternion bodyOrientation = SHQuaternion::FromEuler(rigidBody->GetRotation());
SHVec3 linearVelocity = rigidBody->GetLinearVelocity();
SHVec3 angularVelocity = rigidBody->GetAngularVelocity();
float invMass = 1.0f / rigidBody->GetMass();
SHVec3 localInvInertia = rigidBody->rigidBody->getLocalInertiaTensor();
float invMass = 1.0f / ghostBody.mass;
SHVec3 worldInvInertia = SHVec3::One;
SHVec3 localCentroid = rigidBody->rigidBody->getLocalCenterOfMass();
SHVec3 worldCentroid = SHVec3::One;
SHVec3 appliedForce = simInfo.force;
SHVec3 appliedTorque = simInfo.torque;
SHVec3 accumulatedForce = SHVec3::Zero;
SHVec3 accumulatedTorque = SHVec3::Zero;
const SHVec3& LINEAR_LOCK = rigidBody->rigidBody->getLinearLockAxisFactor();
const SHVec3& ANGULAR_LOCK = rigidBody->rigidBody->getAngularLockAxisFactor();
// Invert the inertia
for (size_t i = 0; i < SHVec3::SIZE; ++i)
localInvInertia[i] = 1.0f / localInvInertia[i];
// Asserts. Don't be an idiot.
SHASSERT(invMass > 0, "GhostBody's mass in invalid")
// Build raycast layer from colliders. If none exist....then this never stops simulating technically.
// I'm too lazy to handle that case, so I'll just throw an error.
@ -302,24 +279,24 @@ namespace SHADE
int iterationCounter = simInfo.maxSteps;
do
{
raycastInfo.distance = linearVelocity.Length();
raycastInfo.ray.position = bodyPosition;
raycastInfo.ray.direction = SHVec3::Normalise(linearVelocity);
raycastInfo.distance = ghostBody.linearVelocity.Length() * simInfo.timeStep; // Do not take the entire velocity's length as that is for an entire second.
raycastInfo.ray.position = ghostBody.position;
raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity);
terminate = !Raycast(raycastInfo).empty() || iterationCounter == 0;
if (terminate)
return;
// Compute world space data
const SHMatrix R = SHMatrix::Rotate(bodyOrientation);
const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation);
const SHMatrix RT = SHMatrix::Transpose(R);
SHMatrix localInertiaTensor = SHMatrix::Identity;
// Set the diagonals
localInertiaTensor.m[0][0] = localInvInertia.x;
localInertiaTensor.m[1][1] = localInvInertia.y;
localInertiaTensor.m[2][2] = localInvInertia.z;
localInertiaTensor.m[0][0] = ghostBody.localInvInertia.x;
localInertiaTensor.m[1][1] = ghostBody.localInvInertia.y;
localInertiaTensor.m[2][2] = ghostBody.localInvInertia.z;
localInertiaTensor *= RT;
const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] };
@ -327,42 +304,47 @@ namespace SHADE
worldInvInertia = R * DIAGONALS;
// Compute world centroid
worldCentroid = (R * localCentroid) + bodyPosition;
worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position;
// Apply forces
accumulatedForce += appliedForce;
angularVelocity += worldInvInertia * SHVec3::Cross(bodyPosition + simInfo.forceOffset, simInfo.force);
accumulatedTorque += appliedTorque;
ghostBody.accumulatedForce += simInfo.force;
ghostBody.angularVelocity += worldInvInertia * SHVec3::Cross(ghostBody.position + simInfo.forceOffset, simInfo.force);
ghostBody.accumulatedTorque += simInfo.torque;
// Integrate Velocities
// Integrate forces and gravity into linear velocity
const SHVec3 LINEAR_ACCELERATION = accumulatedForce * invMass;
const SHVec3 GRAVITATIONAL_ACCELERATION = rigidBody->IsGravityEnabled() ? worldState.settings.gravity * rigidBody->GetGravityScale() : SHVec3::Zero;
const SHVec3 LINEAR_ACCELERATION = ghostBody.accumulatedForce * invMass;
const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero;
linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * LINEAR_LOCK;
angularVelocity += worldInvInertia * (accumulatedTorque * simInfo.timeStep);
ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * ghostBody.linearLock;
ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * simInfo.timeStep);
// Apply drag (exponentially applied)
linearVelocity *= 1.0f / (1.0f + simInfo.timeStep * rigidBody->drag);
angularVelocity *= 1.0f / (1.0f + simInfo.timeStep * rigidBody->angularDrag);
ghostBody.linearVelocity *= 1.0f / (1.0f + simInfo.timeStep * ghostBody.drag);
ghostBody.angularVelocity *= 1.0f / (1.0f + simInfo.timeStep * ghostBody.angularDrag);
// Integrate Positions & Orientations
const SHQuaternion QV = SHQuaternion{ angularVelocity.x * simInfo.timeStep, angularVelocity.y * simInfo.timeStep, angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f;
const SHQuaternion QV = SHQuaternion{ ghostBody.angularVelocity.x * simInfo.timeStep, ghostBody.angularVelocity.y * simInfo.timeStep, ghostBody.angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f;
bodyPosition += linearVelocity * simInfo.timeStep;
bodyOrientation += bodyOrientation * QV * SHQuaternion::FromEuler(ANGULAR_LOCK);
bodyOrientation = SHQuaternion::Normalise(bodyOrientation);
ghostBody.position += ghostBody.linearVelocity * simInfo.timeStep;
ghostBody.orientation += ghostBody.orientation * QV * SHQuaternion::FromEuler(ghostBody.angularLock);
ghostBody.orientation = SHQuaternion::Normalise(ghostBody.orientation);
// Clear forces
ghostBody.accumulatedForce = SHVec3::Zero;
ghostBody.accumulatedTorque = SHVec3::Zero;
// Clear forces after the first frame
if (!simInfo.continuousForce)
{
accumulatedForce = SHVec3::Zero;
accumulatedTorque = SHVec3::Zero;
appliedForce = SHVec3::Zero;
appliedTorque = SHVec3::Zero;
simInfo.force = SHVec3::Zero;
simInfo.torque = SHVec3::Zero;
}
positions.emplace_back(bodyPosition);
if (output.positions)
output.positions->emplace_back(ghostBody.position);
if (output.orientations)
output.orientations->emplace_back(ghostBody.orientation);
--iterationCounter;

View File

@ -28,6 +28,7 @@
namespace SHADE
{
struct SHGhostBody;
/*-----------------------------------------------------------------------------------*/
/* Type Definitions */
/*-----------------------------------------------------------------------------------*/
@ -51,40 +52,54 @@ namespace SHADE
/**
* @brief
* Used to simulate the motion of a rigid body, ignoring collision detection and response.
* @param bodyEID
* The EntityID of the Rigid Body to simulate.
* @param force
* The force applied onto the Rigid Body.
* @param forceOffset
* The position to apply the force onto the body relative to it's local Center of Mass.
* @param torque
* The torque to apply onto the Rigid Body.
* @param continuousForce
* If the force should be applied every step throughout the simulation. Defaults to false. <br/>
* True : The force indicated is added to the body every step, therefore it has constant acceleration.
* False: The force is applied only in the first step, therefore it has constant speed.
* @param timeStep
* The timestep for each step of the simulation. Defaults to 0.016s (The default Fixed DT)
* @param maxSteps
* The number of steps to run the simulation for. Defaults to -1.
* < 0 : Runs until the object may hit something. Hit detection is done through raycasting.
* = 0 : Runs only the current step and checks if it may hit something.
* > 0 : Runs for the given number of steps or until it may hit something.
*/
struct SimulateBodyInfo
{
public:
// The EntityID of the Actual Rigid Body to simulate. If none is passed it,
// the Ghost Body will attempt to collide with everything.
EntityID bodyEID = MAX_EID;
// The force applied onto the Ghost Body.
SHVec3 force = SHVec3::Zero;
// The position where the force is applied offset from the local centroid.
SHVec3 forceOffset = SHVec3::Zero;
// The torque to apply onto the Ghost Body.
SHVec3 torque = SHVec3::Zero;
// Whether or not to clear the force after the first iteration
/*
If the force should be applied every step throughout the simulation. Defaults to false.
True : The force indicated is added to the body every step, therefore it has constant acceleration.
False: The force is applied only in the first step, therefore it has constant speed.
*/
bool continuousForce = false;
// The timestep for each step of the simulation. Defaults to 0.016s (The default Fixed DT)
float timeStep = static_cast<float>(SHPhysicsConstants::DEFAULT_FIXED_DT);
/*
The number of steps to run the simulation for. Defaults to -1.
< 0 : Runs until the object may hit something. Hit detection is done through raycasting.
= 0 : Runs only the current step and checks if it may hit something.
> 0 : Runs for the given number of steps or until it may hit something.
*/
int maxSteps = -1;
};
/**
* @brief
* Contains the output for the simulate body method.
*/
struct SimulateBodyOutput
{
public:
std::vector<SHVec3>* positions = nullptr;
std::vector<SHQuaternion>* orientations = nullptr;
};
/*---------------------------------------------------------------------------------*/
/* Constructors & Destructor */
@ -158,15 +173,16 @@ namespace SHADE
/**
* @brief
* Simulates the motion of a body until it collides with something.
* @param positions
* The output vector for the position of the body in each timestep.
* @param orientations
* The output vector for the orientations of the body in each timestep.
* Simulates a non-existent body in the physics world.
* The simulation will run based on the information passed in.
* @param ghostBody
* The definition of the body passed in.
* @param simInfo
* The information for simulating the body.
* The information for how the simulation will run.
* @param output
* The transform results for position and orientations.
*/
void SimulateBody(std::vector<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo);
void SimulateBody(SHGhostBody& ghostBody, SimulateBodyInfo& simInfo, SimulateBodyOutput& output);
/*---------------------------------------------------------------------------------*/
/* System Routines */