fencer/src/collision.c

#include "collision.h"
#include "debug.h"
#include "player.h"
#include "vmath.h"

// =====================================================
// Shape overlap test using the separating axis theorem
// =====================================================

typedef struct Range {float min; Vector minpoint; float max; Vector maxpoint; } Range;

static
Range _internal_collision_get_range_on_axis(PhysicsEntity self, Vector axis) {
    Transform* transform = self.transformable->get_transform(self.data);
    Shape* shape = self.tc->get_shape(self.data);
    Vector point = shape_get_point_transformed(shape, 0, *transform);
    float dot = vdotf(axis, point);
    Range range = {dot, point, dot, point};

    for(size_t point_index = 1; point_index < shape_get_points_count(shape); ++point_index) {
        point = shape_get_point_transformed(shape, point_index, *transform);
        dot = vdotf(axis, point);
        if(dot < range.min) {
            range.min = dot;
            range.minpoint = point;
        }
        if(dot > range.max) {
            range.max = dot;
            range.maxpoint = point;
        }
    }

    return range;
}

static
Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis) {
    Range a_range = _internal_collision_get_range_on_axis(self, axis);
    Range b_range = _internal_collision_get_range_on_axis(other, axis);

    if(a_range.min <= b_range.max && a_range.min <= b_range.max) {
        return vmulff(axis, fminf(a_range.max - b_range.min, b_range.min - a_range.max));
    } else {
        return ZeroVector;
    }
}

static
int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Collision* out) {
    // get components used
    Shape* self_shape = self.tc->get_shape(self.data);
    Transform* self_transform = self.transformable->get_transform(self.data);

    // the shortest distance to solve collision found so far
    Vector shortest_escape = InfinityVector;
    // the squared length of the shortest escape vector found so far
    float shortest_sqrmag = INFINITY;
    // the first index of the points on the edge
    size_t shortest_escape_edge = 0;
    // the number of points in the shape of self
    size_t self_point_count = shape_get_points_count(self_shape);

    for(size_t point_index = 0; point_index < self_point_count; ++point_index) {
        // the next point on the line
        size_t next_index = (point_index + 1) % self_point_count;
        // get the two points defining the collision edge
        Vector a = shape_get_point_transformed(self.tc->get_shape(self.data), point_index, *self_transform);
        Vector b = shape_get_point_transformed(self.tc->get_shape(self.data), next_index,  *self_transform);
        // the direction of the line
        Vector normal = vperpendicularf(vsubf(b, a));

        // the smallest escape vector on this axis
        Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal));
        float sqr_mag = vsqrmagnitudef(escape);
        if(sqr_mag < shortest_sqrmag) {
            shortest_sqrmag = sqr_mag;
            shortest_escape = escape;
            shortest_escape_edge = point_index;
        }

        if(sqr_mag == 0) {
            out->penetration_vector = InfinityVector;
            return 0;
        }
    }

    RigidBody* rba = self.tc->get_rigidbody(self.data);
    RigidBody* rbb = other.tc->get_rigidbody(other.data);
    Vector velocity = rigidbody_get_velocity(rba);
    velocity = vsubf(velocity, rigidbody_get_velocity(rbb));

    *out = (Collision) {
        .other = other,

        .point = InfinityVector,
        .normal = vnormalizedf(shortest_escape),

        .velocity = velocity,
        .penetration_vector = shortest_escape,

        .edge_left = shape_get_point_transformed(self_shape, shortest_escape_edge, *self_transform),
        .edge_right = shape_get_point_transformed(self_shape, (1 + shortest_escape_edge) % self_point_count, *self_transform),
    };

    return !veqf(shortest_escape, ZeroVector);
}

static
Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) {
    return (Collision){
        .other = a,
        .point = collision_a.point,
        .normal = vinvf(collision_a.normal),
        .velocity = vinvf(collision_a.velocity),
        .penetration_vector = vinvf(collision_a.penetration_vector),
        .edge_left = collision_a.edge_left,
        .edge_right = collision_a.edge_right,
    };
}

int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collision* out_b) {
    Collision collision_a, collision_b;
    int collision_a_overlaps = _internal_collision_get_overlap(a, b, &collision_a);
    int collision_b_overlaps = _internal_collision_get_overlap(b, a, &collision_b);

    if(!collision_a_overlaps || !collision_b_overlaps)
        return 0;

    if(vsqrmagnitudef(collision_a.penetration_vector) < vsqrmagnitudef(collision_b.penetration_vector))
        collision_b = _internal_collision_invert(collision_a, a);
    else
        collision_a = _internal_collision_invert(collision_b, b);

    *out_a = collision_a;
    *out_b = collision_b;
    return (collision_b_overlaps << 1) | collision_a_overlaps;
}