collision_utils.cpp

#include "collision_utils.hpp"

namespace collision_utils {

glm::vec3 compute_normal(const glm::vec3 &a, const glm::vec3 &b, const glm::vec3 &c) {
    return glm::normalize(glm::cross(b - a, c - a));
}

CollisionManifold detect_convex_collision(const draw_info::IndexedVertexPositions &a, const glm::vec3 &posA,
                                          const draw_info::IndexedVertexPositions &b, const glm::vec3 &posB) {
    GlobalLogSection _("detect_convex_collision");
    CollisionManifold manifold;
    float min_overlap = std::numeric_limits<float>::max();
    glm::vec3 face_normal_that_generates_least_overlap;
    Symbol object_that_has_face_normal_geneating_least_overlap;

    std::string name_of_object_a = a.name.empty() ? "<unnamed>" : a.name;
    std::string name_of_object_b = b.name.empty() ? "<unnamed>" : b.name;

    // Log which objects are being tested
    if (!a.name.empty() || !b.name.empty()) {
        global_logger->debug("starting collision detection between a: '{}' with pos: {} and b: '{}' with pos {}",
                             name_of_object_a, vec3_to_string(posA), name_of_object_b, vec3_to_string(posB));
    }

    struct FaceNormal {
        Symbol object;
        glm::vec3 normal;
    };

    std::vector<FaceNormal> normals_of_every_face_on_each_object;

    // 1. add face normals from a
    for (size_t i = 0; i + 2 < a.indices.size(); i += 3) {
        glm::vec3 n = compute_normal(a.xyz_positions[a.indices[i]], a.xyz_positions[a.indices[i + 1]],
                                     a.xyz_positions[a.indices[i + 2]]);
        normals_of_every_face_on_each_object.push_back({Symbol::a, n});
    }

    // 2. add face normals from b
    for (size_t i = 0; i + 2 < b.indices.size(); i += 3) {
        glm::vec3 n = compute_normal(b.xyz_positions[b.indices[i]], b.xyz_positions[b.indices[i + 1]],
                                     b.xyz_positions[b.indices[i + 2]]);
        normals_of_every_face_on_each_object.push_back({Symbol::b, n});
    }

    // 3. test each axis
    for (size_t face_normal_idx = 0; face_normal_idx < normals_of_every_face_on_each_object.size(); ++face_normal_idx) {
        const auto &face_normal = normals_of_every_face_on_each_object[face_normal_idx];

        float a_min_projection = std::numeric_limits<float>::max();
        float a_max_projection = -std::numeric_limits<float>::max();
        for (auto &v : a.xyz_positions) {

            // recall that the dot product is ||a|| * ||b|| cos(theta)
            // where theta is the angle between the vectors a and b, if we suppose that b is a normal vector then
            // the equation becomes
            // ||a|| cos(theta), Since we know that cos(theta) = adj/hyp, and if we drop a perpendicular from a onto
            // the ray generated by b, then we can deduce that adj of that triangle equals hyp * cos(theta) and here
            // we know that ||a|| = hyp, therefore dot(v, face_normal) is the projection of the vertex v along the
            // normal vector's generated line

            float projection_along_line_generated_by_face_normal = glm::dot(v + posA, face_normal.normal);
            a_min_projection = std::min(a_min_projection, projection_along_line_generated_by_face_normal);
            a_max_projection = std::max(a_max_projection, projection_along_line_generated_by_face_normal);
        }

        float b_min_projection = std::numeric_limits<float>::max();
        float b_max_projection = -std::numeric_limits<float>::max();
        for (auto &v : b.xyz_positions) {
            float projection_along_line_generated_by_face_normal = glm::dot(v + posB, face_normal.normal);
            b_min_projection = std::min(b_min_projection, projection_along_line_generated_by_face_normal);
            b_max_projection = std::max(b_max_projection, projection_along_line_generated_by_face_normal);
        }

        float overlap = std::min(a_max_projection, b_max_projection) - std::max(a_min_projection, b_min_projection);

        global_logger->debug(
            "face normal plane generated by object {} with idx: {} and value {}, projections -> A: [{}, {}], B: "
            "[{}, {}], overlap = {}",
            face_normal.object == Symbol::a ? "a" : "b", face_normal_idx, vec3_to_string(face_normal.normal),
            a_min_projection, a_max_projection, b_min_projection, b_max_projection, overlap);

        if (overlap <= 0.0f) {
            global_logger->debug("separating axis found, no collision.");
            return {}; // no collision
        } else {
            if (!a.name.empty() || !b.name.empty()) {
                global_logger->debug("collision along an face plane detected between '{}' and '{}', note this does not "
                                     "mean the two objects collide",
                                     name_of_object_a, name_of_object_b);
            }
        }

        if (overlap < min_overlap) {
            min_overlap = overlap;
            face_normal_that_generates_least_overlap = face_normal.normal;
            object_that_has_face_normal_geneating_least_overlap = face_normal.object;
        }
    }

    manifold.normal_generating_least_overlap = face_normal_that_generates_least_overlap;
    manifold.object_that_normal_exists_on = object_that_has_face_normal_geneating_least_overlap;
    manifold.penetration = min_overlap;

    global_logger->debug("Collision detected: normal = {}, penetration = {}",
                         vec3_to_string(manifold.normal_generating_least_overlap), manifold.penetration);

    return manifold;
}

// TODO: in future use enum rather than string axis name.
void resolve_aabb_collision_on_axis(float &position, float &velocity, float min_a, float max_a, float min_b,
                                    float max_b, const std::string &axis_name) {
    GlobalLogSection _("resolve_aabb_collision_on_axis", LogSection::LogMode::enable);
    if (velocity > 0.0f) {
        // moving positive direction
        float penetration = max_a - min_b;
        global_logger->debug("collision on {} axis (positive): pos = {}, vel = {}, penetration = {}", axis_name,
                             position, velocity, penetration);

        position -= penetration;
        velocity = 0.0f;

        global_logger->debug("resolved collision on {} axis: new pos = {}, new vel = {}", axis_name, position,
                             velocity);

    } else if (velocity < 0.0f) {
        // moving negative direction
        float penetration = max_b - min_a;
        global_logger->debug("collision on {} axis (negative): pos = {}, vel = {}, penetration = {}", axis_name,
                             position, velocity, penetration);

        position += penetration;
        velocity = 0.0f;

        global_logger->debug("resolved collision on {} axis: new pos = {}, new vel = {}", axis_name, position,
                             velocity);
    }
}

} // namespace collision_utils