#include "collision.h" #include "vmath.h" #include "rigidbody.h" #include "collider.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(PhysicsQuery self, Vector axis) { Transform* transform = self.transform; Shape* shape = self.shape; 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(PhysicsQuery self, PhysicsQuery other, Vector axis, Vector* out_point) { Range a_range = _internal_collision_get_range_on_axis(self, axis); Range b_range = _internal_collision_get_range_on_axis(other, axis); const float overlap_left = a_range.max - b_range.min; const float overlap_right = b_range.min - a_range.max; *out_point = fabsf(overlap_right) <= fabsf(overlap_left) ? a_range.maxpoint : a_range.minpoint; if(a_range.min <= b_range.max && b_range.min <= a_range.max) { const float shortest = fminf(overlap_left, overlap_right); return vmulff(axis, shortest); } else { return ZeroVector; } } static int _internal_collision_get_collisions(Collider* self, Collider* other, Collision* out) { // get components used Shape* self_shape = collider_get_shape(self); Transform* self_transform = rigidbody_get_transform(collider_get_rigidbody(self)); PhysicsQuery self_query = { .shape = self_shape, .transform = self_transform, .mask = 0x0 // not used }; PhysicsQuery other_query = { .shape = collider_get_shape(other), .transform = rigidbody_get_transform(collider_get_rigidbody(other)), .mask = 0x0 // not used }; // 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_dot = 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 edge_lhs = shape_get_point_transformed(self_shape, point_index, *self_transform); Vector edge_rhs = shape_get_point_transformed(self_shape, next_index, *self_transform); // the direction of the line Vector normal = vnormalizedf(vperpendicularf(vsubf(edge_rhs, edge_lhs))); Vector overlap_point; // the smallest escape vector on this axis Vector escape = _internal_collision_overlap_on_axis(self_query, other_query, normal, &overlap_point); float dot = vdotf(vinvf(normal), escape); if(dot <= 0.0) { return 0; } if(dot <= shortest_dot) { shortest_dot = dot; shortest_escape = escape; shortest_escape_edge = point_index; } } RigidBody* rba = collider_get_rigidbody(self); RigidBody* rbb = collider_get_rigidbody(other); const Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb)); const Vector normal = vnormalizedf(shortest_escape); Vector world_point = _internal_collision_get_range_on_axis(self_query, normal).minpoint; *out = (Collision) { .other = other, .point = inverse_transform_point(rigidbody_get_transform(rba), world_point), .normal = normal, .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); } Collision collision_invert(Collision collision_a, Collider* a) { RigidBody* body = collider_get_rigidbody(a); Shape* shape = collider_get_shape(a); Transform* transform = rigidbody_get_transform(body); Vector world_point = _internal_collision_get_range_on_axis((PhysicsQuery){.shape = shape, .transform = transform }, collision_a.normal).maxpoint; return (Collision){ .other = a, .point = inverse_transform_point(rigidbody_get_transform(body), world_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(Collider* a, Collider* b, Collision* out_a, Collision* out_b) { if(!(collider_get_layers(a) & collider_get_mask(b)) || !(collider_get_mask(a) & collider_get_layers(b))) return 0; Collision collision_a, collision_b; int collision_a_overlaps = _internal_collision_get_collisions(a, b, &collision_a); int collision_b_overlaps = _internal_collision_get_collisions(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 = collision_invert(collision_a, a); else collision_a = collision_invert(collision_b, b); *out_a = collision_a; *out_b = collision_b; return (collision_b_overlaps << 1) | collision_a_overlaps; } static int _internal_overlap_check(PhysicsQuery a, PhysicsQuery b) { Shape* shape = a.shape; Transform* transform = a.transform; const size_t shape_point_count = shape_get_points_count(shape); for(size_t point_index = 0; point_index < shape_point_count; ++point_index) { // the next point on the line size_t next_index = (point_index + 1) % shape_point_count; // get the two points defining the collision edge Vector edge_lhs = shape_get_point_transformed(shape, point_index, *transform); Vector edge_rhs = shape_get_point_transformed(shape, next_index, *transform); // the direction of the line Vector normal = vnormalizedf(vperpendicularf(vsubf(edge_rhs, edge_lhs))); Vector overlap_point; // the smallest escape vector on this axis Vector escape = _internal_collision_overlap_on_axis(a, b, normal, &overlap_point); float dot = vdotf(vinvf(normal), escape); if(dot <= 0.0) { return 0; } } return 1; } int overlap_check(PhysicsQuery query, Collider* collider) { PhysicsQuery collider_query = collider_to_query(collider); return (query.mask & collider_get_layers(collider)) != 0 && (_internal_overlap_check(query, collider_query) || _internal_overlap_check(collider_query, query)); }