Contact table refactor
This commit is contained in:
parent
8cde9818f4
commit
b085e7ec6a
|
|
@ -10,6 +10,7 @@ typedef enum : uint8_t {
|
|||
TYPE_EMPTY = 0,
|
||||
TYPE_CIRCLE = 1,
|
||||
TYPE_PLANE = 2,
|
||||
TYPE_COUNT,
|
||||
} body_type;
|
||||
|
||||
////////// Types
|
||||
|
|
@ -36,6 +37,15 @@ typedef struct {
|
|||
typedef size_t rigid_body_index;
|
||||
typedef void (*rigid_body_collision_callback_t)(rigid_body_index a, rigid_body_index b);
|
||||
|
||||
// `normal` points from body a toward body b; `overlap` is the penetration depth.
|
||||
typedef struct {
|
||||
bool hit;
|
||||
vec2 normal;
|
||||
float overlap;
|
||||
} contact;
|
||||
|
||||
typedef contact (*contact_fn)(const rigid_body* a, const rigid_body* b);
|
||||
|
||||
constexpr float COLLISION_EPSILON = 1e-6f;
|
||||
|
||||
////////// Prototypes
|
||||
|
|
@ -226,87 +236,112 @@ void rigid_body_resolve_collision(rigid_body_index idx) {
|
|||
}
|
||||
}
|
||||
|
||||
void rigid_body_handle_collision(rigid_body_index idx1, rigid_body_index idx2) {
|
||||
rigid_body* rb1 = rb_get(idx1);
|
||||
rigid_body* rb2 = rb_get(idx2);
|
||||
if (rb1->type == TYPE_CIRCLE && rb2->type == TYPE_CIRCLE) {
|
||||
vec2 d = vec2_sub(rb2->pos, rb1->pos);
|
||||
float distance = vec2_mag(d);
|
||||
////////// Contact generators
|
||||
|
||||
float overlap = rb1->circle.radius + rb2->circle.radius - distance;
|
||||
if (overlap < 0) {
|
||||
return;
|
||||
}
|
||||
static contact contact_circle_circle(const rigid_body* a, const rigid_body* b) {
|
||||
vec2 d = vec2_sub(b->pos, a->pos);
|
||||
float distance = vec2_mag(d);
|
||||
|
||||
collision_event_push(idx1, idx2);
|
||||
|
||||
// Coincident centres have no separation axis; pick an arbitrary one.
|
||||
vec2 n = (distance > COLLISION_EPSILON) ? vec2_div(d, distance) : (vec2){1.0f, 0.0f};
|
||||
|
||||
float factor = (isinf(rb1->mass) || isinf(rb2->mass)) ? 1 : 0.5;
|
||||
vec2 n_overlap = vec2_mul(n, overlap * factor);
|
||||
|
||||
if (!isinf(rb1->mass)) {
|
||||
rb1->pos = vec2_sub(rb1->pos, n_overlap);
|
||||
}
|
||||
if (!isinf(rb2->mass)) {
|
||||
rb2->pos = vec2_add(rb2->pos, n_overlap);
|
||||
}
|
||||
|
||||
float v1n = vec2_dot(rb1->vel, n);
|
||||
float v2n = vec2_dot(rb2->vel, n);
|
||||
|
||||
// Skip the impulse if the bodies are already separating along the normal.
|
||||
if (v1n - v2n <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
float v1n_new = v1n;
|
||||
float v2n_new = v2n;
|
||||
|
||||
if (!isinf(rb1->mass) && !isinf(rb2->mass)) {
|
||||
// Elastic collision: each body weighted by the *other* body's mass.
|
||||
v1n_new = v1n - 2.0f * (v1n - v2n) * rb2->mass / (rb1->mass + rb2->mass);
|
||||
v2n_new = v2n + 2.0f * (v1n - v2n) * rb1->mass / (rb1->mass + rb2->mass);
|
||||
} else if (isinf(rb1->mass)) {
|
||||
v2n_new = -v2n;
|
||||
} else if (isinf(rb2->mass)) {
|
||||
v1n_new = -v1n;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
vec2 v1t = vec2_sub(rb1->vel, vec2_mul(n, v1n));
|
||||
vec2 v2t = vec2_sub(rb2->vel, vec2_mul(n, v2n));
|
||||
|
||||
if (!isinf(rb1->mass)) {
|
||||
rb1->vel = vec2_add(v1t, vec2_mul(n, v1n_new));
|
||||
}
|
||||
if (!isinf(rb2->mass)) {
|
||||
rb2->vel = vec2_add(v2t, vec2_mul(n, v2n_new));
|
||||
}
|
||||
} else if (rb1->type == TYPE_PLANE && rb2->type == TYPE_CIRCLE) {
|
||||
float distance = point_to_line_dist(rb2->pos, rb1->pos, rb1->plane.normal) - rb2->circle.radius;
|
||||
|
||||
float overlap = -distance;
|
||||
if (overlap < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
collision_event_push(idx1, idx2);
|
||||
|
||||
vec2 n = rb1->plane.normal;
|
||||
vec2 n_overlap = vec2_mul(n, overlap);
|
||||
|
||||
rb2->pos = vec2_add(rb2->pos, n_overlap);
|
||||
|
||||
// Reflect only if the circle is moving into the plane.
|
||||
if (vec2_dot(rb2->vel, n) < 0) {
|
||||
rb2->vel = vec2_reflect(rb2->vel, n);
|
||||
}
|
||||
} else if (rb1->type == TYPE_CIRCLE && rb2->type == TYPE_PLANE) {
|
||||
rigid_body_handle_collision(idx2, idx1);
|
||||
float overlap = a->circle.radius + b->circle.radius - distance;
|
||||
if (overlap < 0) {
|
||||
return (contact){.hit = false};
|
||||
}
|
||||
|
||||
// Coincident centres have no separation axis; pick an arbitrary one.
|
||||
vec2 n = (distance > COLLISION_EPSILON) ? vec2_div(d, distance) : (vec2){1.0f, 0.0f};
|
||||
return (contact){.hit = true, .normal = n, .overlap = overlap};
|
||||
}
|
||||
|
||||
static contact contact_plane_circle(const rigid_body* a, const rigid_body* b) {
|
||||
// a: plane (static), b: circle.
|
||||
float distance = point_to_line_dist(b->pos, a->pos, a->plane.normal) - b->circle.radius;
|
||||
|
||||
float overlap = -distance;
|
||||
if (overlap < 0) {
|
||||
return (contact){.hit = false};
|
||||
}
|
||||
|
||||
return (contact){.hit = true, .normal = a->plane.normal, .overlap = overlap};
|
||||
}
|
||||
|
||||
// Indexed [a->type][b->type] in canonical order (a->type >= b->type); a NULL entry
|
||||
// means the pair does not collide.
|
||||
static contact_fn contact_table[TYPE_COUNT][TYPE_COUNT] = {
|
||||
[TYPE_CIRCLE][TYPE_CIRCLE] = contact_circle_circle,
|
||||
[TYPE_PLANE][TYPE_CIRCLE] = contact_plane_circle,
|
||||
};
|
||||
|
||||
// `n` points from a toward b; infinite-mass bodies are treated as immovable.
|
||||
static void resolve_contact(rigid_body* a, rigid_body* b, vec2 n, float overlap) {
|
||||
float factor = (isinf(a->mass) || isinf(b->mass)) ? 1 : 0.5;
|
||||
vec2 n_overlap = vec2_mul(n, overlap * factor);
|
||||
|
||||
if (!isinf(a->mass)) {
|
||||
a->pos = vec2_sub(a->pos, n_overlap);
|
||||
}
|
||||
if (!isinf(b->mass)) {
|
||||
b->pos = vec2_add(b->pos, n_overlap);
|
||||
}
|
||||
|
||||
float van = vec2_dot(a->vel, n);
|
||||
float vbn = vec2_dot(b->vel, n);
|
||||
|
||||
// Skip the impulse if the bodies are already separating along the normal.
|
||||
if (van - vbn <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
float van_new = van;
|
||||
float vbn_new = vbn;
|
||||
|
||||
if (!isinf(a->mass) && !isinf(b->mass)) {
|
||||
// Elastic collision: each body weighted by the *other* body's mass.
|
||||
van_new = van - 2.0f * (van - vbn) * b->mass / (a->mass + b->mass);
|
||||
vbn_new = vbn + 2.0f * (van - vbn) * a->mass / (a->mass + b->mass);
|
||||
} else if (isinf(a->mass)) {
|
||||
vbn_new = -vbn;
|
||||
} else if (isinf(b->mass)) {
|
||||
van_new = -van;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
vec2 vat = vec2_sub(a->vel, vec2_mul(n, van));
|
||||
vec2 vbt = vec2_sub(b->vel, vec2_mul(n, vbn));
|
||||
|
||||
if (!isinf(a->mass)) {
|
||||
a->vel = vec2_add(vat, vec2_mul(n, van_new));
|
||||
}
|
||||
if (!isinf(b->mass)) {
|
||||
b->vel = vec2_add(vbt, vec2_mul(n, vbn_new));
|
||||
}
|
||||
}
|
||||
|
||||
void rigid_body_handle_collision(rigid_body_index idx1, rigid_body_index idx2) {
|
||||
// The greater type enum becomes `a` so each pair needs only one table entry; ties
|
||||
// keep the caller's order, preserving the argument order seen by the callback.
|
||||
rigid_body_index ia = idx1;
|
||||
rigid_body_index ib = idx2;
|
||||
if (rb_get(idx2)->type > rb_get(idx1)->type) {
|
||||
ia = idx2;
|
||||
ib = idx1;
|
||||
}
|
||||
|
||||
rigid_body* a = rb_get(ia);
|
||||
rigid_body* b = rb_get(ib);
|
||||
|
||||
contact_fn generate = contact_table[a->type][b->type];
|
||||
if (!generate) {
|
||||
return;
|
||||
}
|
||||
|
||||
contact c = generate(a, b);
|
||||
if (!c.hit) {
|
||||
return;
|
||||
}
|
||||
|
||||
collision_event_push(ia, ib);
|
||||
resolve_contact(a, b, c.normal, c.overlap);
|
||||
}
|
||||
|
||||
float point_to_line_dist(vec2 p, vec2 line_point, vec2 normal) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue