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

Added a simulate body method 

Raycast effectiveness is untested, but simulation logic matches standard 3D rigid body simulation methods
This commit is contained in:
Diren D Bharwani 2023-03-07 18:48:06 +08:00
parent 50dafa555a
commit c36345c60c
4 changed files with 165 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
SHADE_Engine/src/Camera/SHCameraArmComponent.h
View File

@ -9,9 +9,7 @@
namespace SHADE
{
class SHBox;
class SHRay;
struct SHRay;
class SH_API SHCameraArmComponent final: public SHComponent
{

4
SHADE_Engine/src/Physics/SHPhysicsConstants.h
View File

@ -10,14 +10,12 @@
#pragma once
// Project Headers
#include "Math/Vector/SHVec3.h"
namespace SHADE
{
struct SHPhysicsConstants
{
static constexpr double DEFAULT_FIXED_DT = 1.0 / 60.0;
static constexpr double DEFAULT_FIXED_UPDATE_RATE = 60.0;
static constexpr int DEFAULT_VELOCITY_ITERATIONS = 10;
static constexpr int DEFAULT_POSITION_ITERATIONS = 5;

129
SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp
View File

@ -236,6 +236,135 @@ namespace SHADE
return IS_COLLIDING;
}
void SHPhysicsSystem::SimulateBody(std::vector<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo)
{
// 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;
}
// 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();
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];
// 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.
uint16_t raycastLayers = 0;
if (const auto* colliders = SHComponentManager::GetComponent_s<SHColliderComponent>(simInfo.bodyEID); colliders)
{
const auto& shapes = colliders->GetCollisionShapes();
for (auto& shape : shapes)
raycastLayers |= shape->GetCollisionTag().GetMask();
}
else
{
SHLOG_WARNING("Unable to simulate body {} without a collider (it will simulate forever!)", simInfo.bodyEID)
return;
}
// Raycast direction is always in the direction of the linear velocity
// Raycast distance is the length of the velocity
SHRaycaster::RaycastInfo raycastInfo;
raycastInfo.continuous = false;
raycastInfo.layers = raycastLayers;
bool terminate = true;
do
{
raycastInfo.distance = linearVelocity.Length();
raycastInfo.ray.position = bodyPosition;
raycastInfo.ray.direction = SHVec3::Normalise(linearVelocity);
terminate = !Raycast(raycastInfo).empty();
if (terminate)
break;
// Compute world space data
const SHMatrix R = SHMatrix::Rotate(bodyOrientation);
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 *= RT;
const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] };
worldInvInertia = R * DIAGONALS;
// Compute world centroid
worldCentroid = (R * localCentroid) + bodyPosition;
// Apply forces
accumulatedForce += appliedForce;
angularVelocity += worldInvInertia * SHVec3::Cross(bodyPosition + simInfo.forceOffset, simInfo.force);
accumulatedTorque += appliedTorque;
// 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;
linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * LINEAR_LOCK;
angularVelocity += worldInvInertia * (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);
// Integrate Positions & Orientations
const SHQuaternion QV = SHQuaternion{ angularVelocity.x * simInfo.timeStep, angularVelocity.y * simInfo.timeStep, angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f;
bodyPosition += linearVelocity * simInfo.timeStep;
bodyOrientation += bodyOrientation * QV;
bodyOrientation = SHQuaternion::Normalise(bodyOrientation);
// Clear forces after the first frame
if (!simInfo.continuousForce)
{
accumulatedForce = SHVec3::Zero;
accumulatedTorque = SHVec3::Zero;
appliedForce = SHVec3::Zero;
appliedTorque = SHVec3::Zero;
}
positions.emplace_back(bodyPosition);
} while (true);
}
/*-----------------------------------------------------------------------------------*/
/* Private Function Member Definitions */
/*-----------------------------------------------------------------------------------*/

34
SHADE_Engine/src/Physics/System/SHPhysicsSystem.h
View File

@ -44,6 +44,26 @@ namespace SHADE
friend class SHRaycaster;
public:
/*---------------------------------------------------------------------------------*/
/* Type Definitions */
/*---------------------------------------------------------------------------------*/
/**
* @brief
* Used to simulate the motion of a rigid body until it hits something.
*/
struct SimulateBodyInfo
{
EntityID bodyEID = MAX_EID;
SHVec3 force = SHVec3::Zero;
SHVec3 forceOffset = SHVec3::Zero;
SHVec3 torque = SHVec3::Zero;
// Whether or not to clear the force after the first iteration
bool continuousForce = false;
float timeStep = static_cast<float>(SHPhysicsConstants::DEFAULT_FIXED_DT);
};
/*---------------------------------------------------------------------------------*/
@ -51,7 +71,7 @@ namespace SHADE
/*---------------------------------------------------------------------------------*/
SHPhysicsSystem () noexcept;
~SHPhysicsSystem() noexcept;
~SHPhysicsSystem() noexcept override;
/*---------------------------------------------------------------------------------*/
/* Getter Functions */
@ -116,6 +136,18 @@ namespace SHADE
*/
[[nodiscard]] bool TestAABBOverlap(const SHAABB& aabb, uint16_t layers = static_cast<uint16_t>(SHCollisionTag::Layer::ALL));
/**
* @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.
* @param simInfo
* The information for simulating the body.
*/
void SimulateBody(std::vector<SHVec3>& positions, std::vector<SHQuaternion>& orientations, const SimulateBodyInfo& simInfo);
/*---------------------------------------------------------------------------------*/
/* System Routines */
/*---------------------------------------------------------------------------------*/