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

Fixed consistency of Trajectories with different time steps #436

Merged
direnbharwani merged 5 commits from SP3-2-Physics into main 2023-03-22 22:33:24 +08:00
Conversation 0 Commits 5 Files Changed 2 +84 -71
2 changed files with 84 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -239,21 +239,6 @@ namespace SHADE
void SHPhysicsSystem::SimulateBody(SHGhostBody& ghostBody, SimulateBodyInfo& simInfo, SimulateBodyOutput& output) 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! This body will collide with everything!", simInfo.bodyEID)
}
// Prepare simulation info (I'm basically declaring an entire body here)
float invMass = 1.0f / ghostBody.mass;
SHVec3 worldInvInertia = SHVec3::One;
SHVec3 worldCentroid = SHVec3::One;
// 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. // 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. // I'm too lazy to handle that case, so I'll just throw an error.
uint16_t raycastLayers = 0; uint16_t raycastLayers = 0;
@ -275,64 +260,31 @@ namespace SHADE
raycastInfo.continuous = false; raycastInfo.continuous = false;
raycastInfo.layers = raycastLayers; raycastInfo.layers = raycastLayers;
bool terminate = true; // 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! This body will collide with everything!", simInfo.bodyEID)
}
double accumulatedTime = 0.0f;
int iterationCounter = simInfo.maxSteps; int iterationCounter = simInfo.maxSteps;
do do
{ {
accumulatedTime += simInfo.timeStep;
raycastInfo.distance = ghostBody.linearVelocity.Length() * simInfo.timeStep; // Do not take the entire velocity's length as that is for an entire second. 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.position = ghostBody.position;
raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity); raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity);
terminate = !Raycast(raycastInfo).empty() || iterationCounter == 0; if (!Raycast(raycastInfo).empty())
if (terminate)
return; return;
// Compute world space data while (accumulatedTime > fixedDT)
const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation); {
const SHMatrix RT = SHMatrix::Transpose(R); simulateBody(ghostBody, simInfo);
accumulatedTime -= fixedDT;
SHMatrix localInertiaTensor = SHMatrix::Identity;
// Set the diagonals
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] };
worldInvInertia = R * DIAGONALS;
// Compute world centroid
worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position;
// Apply forces
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 = ghostBody.accumulatedForce * invMass;
const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero;
ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * simInfo.timeStep * ghostBody.linearLock;
ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * simInfo.timeStep);
// Apply drag (exponentially applied)
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{ ghostBody.angularVelocity.x * simInfo.timeStep, ghostBody.angularVelocity.y * simInfo.timeStep, ghostBody.angularVelocity.z * simInfo.timeStep, 0.0f } * 0.5f;
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;
if (!simInfo.continuousForce) if (!simInfo.continuousForce)
{ {
@ -340,14 +292,16 @@ namespace SHADE
simInfo.torque = SHVec3::Zero; simInfo.torque = SHVec3::Zero;
} }
if (--iterationCounter == 0)
return;
}
if (output.positions) if (output.positions)
output.positions->emplace_back(ghostBody.position); output.positions->emplace_back(ghostBody.position);
if (output.orientations) if (output.orientations)
output.orientations->emplace_back(ghostBody.orientation); output.orientations->emplace_back(ghostBody.orientation);
--iterationCounter;
} while (true); } while (true);
} }
@ -494,4 +448,61 @@ namespace SHADE
return onComponentRemovedEvent.get()->handle; return onComponentRemovedEvent.get()->handle;
} }
void SHPhysicsSystem::simulateBody(SHGhostBody& ghostBody, const SimulateBodyInfo& simInfo) noexcept
{
float invMass = 1.0f / ghostBody.mass;
SHVec3 worldInvInertia = SHVec3::One;
SHVec3 worldCentroid = SHVec3::One;
SHASSERT(invMass > 0, "GhostBody's mass in invalid")
// Compute world space data
const SHMatrix R = SHMatrix::Rotate(ghostBody.orientation);
const SHMatrix RT = SHMatrix::Transpose(R);
SHMatrix localInertiaTensor = SHMatrix::Identity;
// Set the diagonals
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] };
worldInvInertia = R * DIAGONALS;
// Compute world centroid
worldCentroid = (R * ghostBody.localCentroid) + ghostBody.position;
// Apply forces
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 = ghostBody.accumulatedForce * invMass;
const SHVec3 GRAVITATIONAL_ACCELERATION = ghostBody.gravityScale ? worldState.settings.gravity * ghostBody.gravityScale : SHVec3::Zero;
ghostBody.linearVelocity += (LINEAR_ACCELERATION + GRAVITATIONAL_ACCELERATION) * fixedDT * ghostBody.linearLock;
ghostBody.angularVelocity += worldInvInertia * (ghostBody.accumulatedTorque * fixedDT);
// Apply drag (exponentially applied)
ghostBody.linearVelocity *= 1.0f / (1.0f + fixedDT * ghostBody.drag);
ghostBody.angularVelocity *= 1.0f / (1.0f + fixedDT * ghostBody.angularDrag);
// Integrate Positions & Orientations
const SHQuaternion QV = SHQuaternion{ ghostBody.angularVelocity.x, ghostBody.angularVelocity.y, ghostBody.angularVelocity.z, 0.0f } * fixedDT * 0.5f;
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;
}
} // namespace SHADE } // namespace SHADE

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

@ -274,5 +274,7 @@ namespace SHADE
SHEventHandle onComponentAdded (SHEventPtr onComponentAddedEvent); SHEventHandle onComponentAdded (SHEventPtr onComponentAddedEvent);
SHEventHandle onComponentRemoved (SHEventPtr onComponentRemovedEvent); SHEventHandle onComponentRemoved (SHEventPtr onComponentRemovedEvent);
void simulateBody (SHGhostBody& ghostBody, const SimulateBodyInfo& simInfo) noexcept;
}; };
} // namespace SHADE } // namespace SHADE