// Physics.cpp
#include "Physics.h"
namespace Engine {
namespace Physics {
void PhysicsSystem::Update(float deltaTime) {
// Fixed timestep for stability
const float fixedTimeStep = 1.0f / 60.0f;
// Accumulate forces (e.g., gravity)
for (auto& body : m_RigidBodies) {
if (!body->IsKinematic() && body->IsGravityEnabled()) {
body->m_AccumulatedForce += m_Gravity * body->m_Mass;
}
}
// Integrate forces
for (auto& body : m_RigidBodies) {
if (!body->IsKinematic()) {
IntegrateForces(*body, fixedTimeStep);
}
}
// Detect and resolve collisions
std::vector<CollisionInfo> collisions;
DetectCollisions(collisions);
ResolveCollisions(collisions);
// Integrate velocities
for (auto& body : m_RigidBodies) {
if (!body->IsKinematic()) {
IntegrateVelocities(*body, fixedTimeStep);
}
}
// Clear accumulated forces
for (auto& body : m_RigidBodies) {
body->m_AccumulatedForce = glm::vec3(0.0f);
body->m_AccumulatedTorque = glm::vec3(0.0f);
}
}
void PhysicsSystem::IntegrateForces(RigidBody& body, float deltaTime) {
// Update linear velocity
body.m_LinearVelocity += (body.m_AccumulatedForce * body.m_InverseMass) * deltaTime;
// Update angular velocity
body.m_AngularVelocity += glm::vec3(body.m_InverseInertiaTensor * glm::vec4(body.m_AccumulatedTorque, 0.0f)) * deltaTime;
// Apply damping
const float linearDamping = 0.01f;
const float angularDamping = 0.01f;
body.m_LinearVelocity *= (1.0f - linearDamping);
body.m_AngularVelocity *= (1.0f - angularDamping);
}
void PhysicsSystem::IntegrateVelocities(RigidBody& body, float deltaTime) {
// Update position
body.m_Position += body.m_LinearVelocity * deltaTime;
// Update rotation
glm::quat angularVelocityQuat(0.0f, body.m_AngularVelocity.x, body.m_AngularVelocity.y, body.m_AngularVelocity.z);
body.m_Rotation += (angularVelocityQuat * body.m_Rotation) * 0.5f * deltaTime;
body.m_Rotation = glm::normalize(body.m_Rotation);
}
void PhysicsSystem::DetectCollisions(std::vector<CollisionInfo>& collisions) {
// Simple O(n²) collision detection
for (size_t i = 0; i < m_RigidBodies.size(); i++) {
for (size_t j = i + 1; j < m_RigidBodies.size(); j++) {
auto& bodyA = m_RigidBodies[i];
auto& bodyB = m_RigidBodies[j];
// Skip if both bodies are kinematic
if (bodyA->IsKinematic() && bodyB->IsKinematic()) {
continue;
}
// Skip if either body doesn't have a collider
if (!bodyA->GetCollider() || !bodyB->GetCollider()) {
continue;
}
CollisionInfo info;
if (CheckCollision(*bodyA, *bodyB, info)) {
info.bodyA = bodyA;
info.bodyB = bodyB;
collisions.push_back(info);
}
}
}
}
void PhysicsSystem::ResolveCollisions(std::vector<CollisionInfo>& collisions) {
for (auto& collision : collisions) {
auto bodyA = collision.bodyA;
auto bodyB = collision.bodyB;
// Calculate relative velocity
glm::vec3 relativeVelocity = bodyB->m_LinearVelocity - bodyA->m_LinearVelocity;
// Calculate impulse magnitude
float velocityAlongNormal = glm::dot(relativeVelocity, collision.normal);
// Don't resolve if velocities are separating
if (velocityAlongNormal > 0) {
continue;
}
// Calculate restitution (bounciness)
float restitution = std::min(bodyA->m_Restitution, bodyB->m_Restitution);
// Calculate impulse scalar
float j = -(1.0f + restitution) * velocityAlongNormal;
j /= bodyA->m_InverseMass + bodyB->m_InverseMass;
// Apply impulse
glm::vec3 impulse = collision.normal * j;
if (!bodyA->IsKinematic()) {
bodyA->m_LinearVelocity -= impulse * bodyA->m_InverseMass;
}
if (!bodyB->IsKinematic()) {
bodyB->m_LinearVelocity += impulse * bodyB->m_InverseMass;
}
// Resolve penetration (position correction)
const float percent = 0.2f; // usually 20% to 80%
const float slop = 0.01f; // small penetration allowed
glm::vec3 correction = std::max(collision.penetrationDepth - slop, 0.0f) * percent * collision.normal / (bodyA->m_InverseMass + bodyB->m_InverseMass);
if (!bodyA->IsKinematic()) {
bodyA->m_Position -= correction * bodyA->m_InverseMass;
}
if (!bodyB->IsKinematic()) {
bodyB->m_Position += correction * bodyB->m_InverseMass;
}
}
}
bool PhysicsSystem::CheckCollision(const RigidBody& bodyA, const RigidBody& bodyB, CollisionInfo& info) {
auto colliderA = bodyA.GetCollider();
auto colliderB = bodyB.GetCollider();
if (colliderA->GetType() == ColliderType::Sphere && colliderB->GetType() == ColliderType::Sphere) {
return SphereVsSphere(bodyA, bodyB, info);
}
else if (colliderA->GetType() == ColliderType::Box && colliderB->GetType() == ColliderType::Box) {
return BoxVsBox(bodyA, bodyB, info);
}
else if (colliderA->GetType() == ColliderType::Sphere && colliderB->GetType() == ColliderType::Box) {
return SphereVsBox(bodyA, bodyB, info);
}
else if (colliderA->GetType() == ColliderType::Box && colliderB->GetType() == ColliderType::Sphere) {
bool result = SphereVsBox(bodyB, bodyA, info);
if (result) {
// Flip normal direction
info.normal = -info.normal;
}
return result;
}
// Unsupported collision types
return false;
}
bool PhysicsSystem::SphereVsSphere(const RigidBody& bodyA, const RigidBody& bodyB, CollisionInfo& info) {
auto sphereA = std::static_pointer_cast<SphereCollider>(bodyA.GetCollider());
auto sphereB = std::static_pointer_cast<SphereCollider>(bodyB.GetCollider());
glm::vec3 posA = bodyA.GetPosition() + sphereA->GetOffset();
glm::vec3 posB = bodyB.GetPosition() + sphereB->GetOffset();
float radiusA = sphereA->GetRadius();
float radiusB = sphereB->GetRadius();
glm::vec3 direction = posB - posA;
float distance = glm::length(direction);
float minDistance = radiusA + radiusB;
if (distance >= minDistance) {
return false;
}
// Normalize direction
direction = distance > 0.0001f ? direction / distance : glm::vec3(0, 1, 0);
info.contactPoint = posA + direction * radiusA;
info.normal = direction;
info.penetrationDepth = minDistance - distance;
return true;
}
// Implementation of BoxVsBox and SphereVsBox collision detection would go here
// These are more complex and would require additional helper functions
} // namespace Physics
} // namespace Engine