Destrum/destrum/src/Physics/SimplePhysicsWorld.cpp

#include <destrum/Physics/SimplePhysicsWorld.h>

#include <algorithm>
#include <cmath>
#include <limits>

#include <destrum/Components/Physics/Rigidbody.h>
#include <destrum/ObjectModel/GameObject.h>
#include <destrum/ObjectModel/Transform.h>

SimplePhysicsWorld::SimplePhysicsWorld(const glm::vec3& gravity)
    : m_Gravity(gravity) {
}

PhysicsBodyHandle SimplePhysicsWorld::CreateBody(const PhysicsBodyDesc& desc) {
    BodyRecord record{};
    record.alive = true;
    record.handle = PhysicsBodyHandle{m_NextId++};
    record.desc = desc;
    record.previousTransform = desc.transform;
    record.currentTransform = desc.transform;

    if (desc.owner) {
        record.rigidbody = desc.owner->GetComponent<Rigidbody>();
    }

    m_Bodies.emplace_back(record);
    return record.handle;
}

void SimplePhysicsWorld::DestroyBody(PhysicsBodyHandle body) {
    if (BodyRecord* record = FindBody(body)) {
        record->alive = false;
        record->rigidbody = nullptr;
    }
}

void SimplePhysicsWorld::SetBodyTransform(PhysicsBodyHandle body, const PhysicsTransform& transform) {
    BodyRecord* record = FindBody(body);
    if (!record) {
        return;
    }

    record->previousTransform = record->currentTransform;
    record->currentTransform = transform;
}

PhysicsTransform SimplePhysicsWorld::GetBodyTransform(PhysicsBodyHandle body) const {
    const BodyRecord* record = FindBody(body);
    if (!record) {
        return {};
    }

    return record->currentTransform;
}

void SimplePhysicsWorld::SetLinearVelocity(PhysicsBodyHandle body, const glm::vec3& velocity) {
    BodyRecord* record = FindBody(body);
    if (!record) {
        return;
    }

    record->linearVelocity = velocity;
}

glm::vec3 SimplePhysicsWorld::GetLinearVelocity(PhysicsBodyHandle body) const {
    const BodyRecord* record = FindBody(body);
    if (!record) {
        return glm::vec3{0.0f};
    }

    return record->linearVelocity;
}

void SimplePhysicsWorld::AddForce(PhysicsBodyHandle body, const glm::vec3& force) {
    BodyRecord* record = FindBody(body);
    if (!record || record->desc.type != RigidbodyType::Dynamic) {
        return;
    }

    record->accumulatedForce += force;
}

void SimplePhysicsWorld::AddImpulse(PhysicsBodyHandle body, const glm::vec3& impulse) {
    BodyRecord* record = FindBody(body);
    if (!record || record->desc.type != RigidbodyType::Dynamic) {
        return;
    }

    const float invMass = record->desc.mass > 0.0f ? 1.0f / record->desc.mass : 0.0f;
    record->linearVelocity += impulse * invMass;
}

void SimplePhysicsWorld::Step(float fixedDt) {
    if (fixedDt <= 0.0f) {
        return;
    }

    for (BodyRecord& body : m_Bodies) {
        if (!body.alive) {
            continue;
        }

        body.previousTransform = body.currentTransform;

        if (body.desc.type != RigidbodyType::Dynamic) {
            continue;
        }

        const float invMass = body.desc.mass > 0.0f ? 1.0f / body.desc.mass : 0.0f;

        glm::vec3 acceleration{0.0f};

        if (body.desc.useGravity) {
            acceleration += m_Gravity;
        }

        acceleration += body.accumulatedForce * invMass;

        // Semi-implicit Euler.
        body.linearVelocity += acceleration * fixedDt;
        body.currentTransform.position += body.linearVelocity * fixedDt;

        body.accumulatedForce = glm::vec3{0.0f};
    }

    // Compact dead records occasionally.
    m_Bodies.erase(
        std::remove_if(m_Bodies.begin(), m_Bodies.end(),
            [](const BodyRecord& body) { return !body.alive; }),
        m_Bodies.end());
}

void SimplePhysicsWorld::SyncKinematicBodiesToPhysics() {
    for (BodyRecord& body : m_Bodies) {
        if (!body.alive || body.desc.type != RigidbodyType::Kinematic || !body.desc.owner) {
            continue;
        }

        Transform& transform = body.desc.owner->GetTransform();

        body.previousTransform = body.currentTransform;
        body.currentTransform.position = transform.GetWorldPosition();
        body.currentTransform.rotation = transform.GetWorldRotation();
    }
}

void SimplePhysicsWorld::SyncDynamicBodiesToTransforms() {
    for (BodyRecord& body : m_Bodies) {
        if (!body.alive || body.desc.type != RigidbodyType::Dynamic || !body.desc.owner) {
            continue;
        }

        Transform& transform = body.desc.owner->GetTransform();
        transform.SetWorldPosition(body.currentTransform.position);
        transform.SetWorldRotation(body.currentTransform.rotation);
    }
}

bool SimplePhysicsWorld::Raycast(const glm::vec3& origin,
                                 const glm::vec3& direction,
                                 float maxDistance,
                                 PhysicsRaycastHit& hit) const {
    if (maxDistance <= 0.0f) {
        return false;
    }

    const float len = glm::length(direction);
    if (len <= 0.00001f) {
        return false;
    }

    const glm::vec3 dir = direction / len;

    bool found = false;
    float bestDistance = std::numeric_limits<float>::max();

    for (const BodyRecord& body : m_Bodies) {
        if (!body.alive || body.desc.shape.type == PhysicsShapeType::None) {
            continue;
        }

        float distance = 0.0f;
        const float radius = GetApproxBoundingRadius(body);
        const glm::vec3 center = body.currentTransform.position + body.desc.shape.centerOffset;

        if (RaySphere(origin, dir, center, radius, maxDistance, distance)) {
            if (distance < bestDistance) {
                bestDistance = distance;
                found = true;

                hit.object = body.desc.owner;
                hit.body = body.handle;
                hit.distance = distance;
                hit.point = origin + dir * distance;

                const glm::vec3 n = hit.point - center;
                hit.normal = glm::length(n) > 0.00001f ? glm::normalize(n) : glm::vec3{0.0f, 1.0f, 0.0f};
            }
        }
    }

    return found;
}

SimplePhysicsWorld::BodyRecord* SimplePhysicsWorld::FindBody(PhysicsBodyHandle body) {
    for (BodyRecord& record : m_Bodies) {
        if (record.alive && record.handle == body) {
            return &record;
        }
    }

    return nullptr;
}

const SimplePhysicsWorld::BodyRecord* SimplePhysicsWorld::FindBody(PhysicsBodyHandle body) const {
    for (const BodyRecord& record : m_Bodies) {
        if (record.alive && record.handle == body) {
            return &record;
        }
    }

    return nullptr;
}

float SimplePhysicsWorld::GetApproxBoundingRadius(const BodyRecord& body) const {
    const PhysicsShapeDesc& shape = body.desc.shape;

    switch (shape.type) {
        case PhysicsShapeType::Box:
            return glm::length(shape.halfExtents);

        case PhysicsShapeType::Sphere:
            return shape.radius;

        case PhysicsShapeType::Capsule:
            return shape.height * 0.5f;

        case PhysicsShapeType::None:
        default:
            return 0.0f;
    }
}

bool SimplePhysicsWorld::RaySphere(const glm::vec3& origin,
                                   const glm::vec3& dirNormalized,
                                   const glm::vec3& center,
                                   float radius,
                                   float maxDistance,
                                   float& outDistance) {
    const glm::vec3 oc = origin - center;

    const float a = glm::dot(dirNormalized, dirNormalized);
    const float b = 2.0f * glm::dot(oc, dirNormalized);
    const float c = glm::dot(oc, oc) - radius * radius;

    const float discriminant = b * b - 4.0f * a * c;
    if (discriminant < 0.0f) {
        return false;
    }

    const float sqrtDisc = std::sqrt(discriminant);
    const float t0 = (-b - sqrtDisc) / (2.0f * a);
    const float t1 = (-b + sqrtDisc) / (2.0f * a);

    float t = t0;
    if (t < 0.0f) {
        t = t1;
    }

    if (t < 0.0f || t > maxDistance) {
        return false;
    }

    outDistance = t;
    return true;
}