BoxCollider.cpp

Go to the documentation of this file.

#include "BoxCollider.h"
 
#include <iostream>
#include <glm/gtx/matrix_decompose.hpp>
 
Physics::Bounding::BoxCollider::BoxCollider(const glm::vec3 &center, const glm::vec3 &halfExtents, const glm::quat &rotation)
    : center(center), halfExtents(halfExtents), rotation(rotation) {}
 
bool Physics::Bounding::BoxCollider::contains(const glm::vec3 &p) const {
    glm::vec3 local = glm::inverse(rotation) * (p - center);
    return glm::all(glm::lessThanEqual(glm::abs(local), halfExtents));
}
 
Physics::Bounding::ContactInfo Physics::Bounding::BoxCollider::closestPoint(const glm::vec3 &p) const {
    glm::vec3 local = glm::inverse(rotation) * (p - center);
    glm::vec3 clamped = glm::clamp(local, -halfExtents, halfExtents);
    glm::vec3 worldPoint = rotation * clamped + center;
 
    glm::vec3 delta = local - clamped;
    float outsideDist = glm::length(delta);
 
    glm::vec3 localNormal;
    float penetration;
    if (outsideDist > 0.0f) {
        // Point is outside: normal points from box toward point
        localNormal = delta;
        penetration = -outsideDist;
    } else {
        glm::vec3 d = halfExtents - glm::abs(local);
        if (d.x <= d.y && d.x <= d.z) {
            localNormal = glm::vec3((local.x > 0.0f) ? 1.0f : -1.0f, 0.0f, 0.0f);
            penetration = d.x;
        } else if (d.y <= d.x && d.y <= d.z) {
            localNormal = glm::vec3(0.0f, (local.y > 0.0f) ? 1.0f : -1.0f, 0.0f);
            penetration = d.y;
        } else {
            localNormal = glm::vec3(0.0f, 0.0f, (local.z > 0.0f) ? 1.0f : -1.0f);
            penetration = d.z;
        }
    }
    glm::vec3 worldNormal = glm::normalize(rotation * localNormal);
 
    return { worldPoint, worldNormal, penetration };
}
 
std::optional<float> Physics::Bounding::BoxCollider::intersectRay(const Math::Ray& ray) const {
    glm::vec3 localOrig = glm::inverse(rotation) * (ray.origin - center);
    glm::vec3 localDir  = glm::inverse(rotation) * ray.dir;
 
    glm::vec3 invDir = 1.0f / localDir;
    glm::vec3 tMinVec = (-halfExtents - localOrig) * invDir;
    glm::vec3 tMaxVec = ( halfExtents - localOrig) * invDir;
 
    glm::vec3 tsmaller = glm::min(tMinVec, tMaxVec);
    glm::vec3 tbigger = glm::max(tMinVec, tMaxVec);
 
    float tmin = std::max(std::max(tsmaller.x, tsmaller.y), tsmaller.z);
    float tmax = std::min(std::min(tbigger.x, tbigger.y), tbigger.z);
 
    bool hit = tmax >= std::max(tmin, 0.0f);
    return hit ? std::optional{ tmin } : std::nullopt;
}
 
std::unique_ptr<Physics::Bounding::ICollider> Physics::Bounding::BoxCollider::getTransformed(const glm::mat4 &modelMatrix) const {
    glm::vec3 skew, translation, scale;
    glm::vec4 persp;
    glm::quat rot;
    glm::decompose(modelMatrix, scale, rot, translation, skew, persp);
 
    return std::make_unique<BoxCollider>(scale * center + translation, halfExtents*scale, rot);
}
 
glm::vec3 Physics::Bounding::BoxCollider::getAABBMin() const {
    glm::mat3 R = glm::mat3_cast(rotation);
    glm::mat3 absR = glm::mat3(
        glm::abs(R[0]),
        glm::abs(R[1]),
        glm::abs(R[2])
    );
    return center - absR * halfExtents;
}
glm::vec3 Physics::Bounding::BoxCollider::getAABBMax() const {
    glm::mat3 R = glm::mat3_cast(rotation);
    glm::mat3 absR = glm::mat3(
        glm::abs(R[0]),
        glm::abs(R[1]),
        glm::abs(R[2])
    );
    return center + absR * halfExtents;
}