diff --git a/src/common/physics/engine.c b/src/common/physics/engine.c index 7287981..8327968 100644 --- a/src/common/physics/engine.c +++ b/src/common/physics/engine.c @@ -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) {