123 lines
4.7 KiB
C
123 lines
4.7 KiB
C
#include "collision.h"
|
|
#include "player.h"
|
|
|
|
// =====================================================
|
|
// Shape overlap test using the separating axis theorem
|
|
// =====================================================
|
|
|
|
typedef struct Range {float min; float max; } Range;
|
|
|
|
static
|
|
Range _internal_collision_get_range_on_axis(PhysicsEntity self, Vector axis) {
|
|
Vector point = shape_get_point_transformed(self.tc->get_shape(self.data), 0, *self.transformable->get_transform(self.data));
|
|
float dot = vdotf(axis, point);
|
|
Range range = {dot, dot};
|
|
|
|
for(size_t point_index = 1; point_index < shape_get_points_count(self.tc->get_shape(self.data)); ++point_index) {
|
|
point = shape_get_point_transformed(self.tc->get_shape(self.data), point_index, *self.transformable->get_transform(self.data));
|
|
dot = vdotf(axis, point);
|
|
if(dot < range.min)
|
|
range.min = fminf(dot, range.min);
|
|
if(dot > range.max)
|
|
range.max = fmaxf(dot, range.max);
|
|
}
|
|
|
|
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 && b_range.min <= b_range.max)
|
|
return vmulff(axis, fminf(b_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 diff = vsubf(b, a);
|
|
|
|
// the smallest escape vector on this axis
|
|
Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(diff));
|
|
float sqr_mag = vsqrmagnitudef(escape);
|
|
if(sqr_mag < shortest_sqrmag) {
|
|
shortest_sqrmag = sqr_mag;
|
|
shortest_escape = escape;
|
|
shortest_escape_edge = point_index;
|
|
}
|
|
}
|
|
|
|
RigidBody* rba = self.tc->get_rigidbody(self.data);
|
|
RigidBody* rbb = self.tc->get_rigidbody(self.data);
|
|
|
|
*out = (Collision) {
|
|
.other = other,
|
|
|
|
.point = InfinityVector,
|
|
.normal = vnormalizedf(shortest_escape),
|
|
|
|
.velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb)),
|
|
.separation_force = 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 shortest_sqrmag != 0;
|
|
}
|
|
|
|
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),
|
|
.separation_force = vinvf(collision_a.separation_force),
|
|
.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.separation_force) < vsqrmagnitudef(collision_b.separation_force)) {
|
|
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 1;
|
|
} |