153 lines
3.2 KiB
C
153 lines
3.2 KiB
C
#include "physics.h"
|
|
|
|
#include "entity.h"
|
|
#include "renderer.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
typedef struct {
|
|
bool valid;
|
|
unsigned component_id;
|
|
physics_t state;
|
|
} entry_t;
|
|
|
|
static unsigned max;
|
|
static unsigned escape_entity;
|
|
static entry_t entries[MAX_ENTITIES];
|
|
|
|
static void update(unsigned new_entity_id, void *ref)
|
|
{
|
|
entry_t *old_e = (entry_t *)ref;
|
|
entry_t *new_e = entries + new_entity_id;
|
|
memcpy(new_e, old_e, sizeof(entry_t));
|
|
old_e->valid = false;
|
|
entity_update_component(new_entity_id, new_e->component_id, new_e);
|
|
}
|
|
|
|
static void remove(void *ref)
|
|
{
|
|
entry_t *e = (entry_t *)ref;
|
|
e->valid = false;
|
|
}
|
|
|
|
void physics_init()
|
|
{
|
|
max = 0;
|
|
escape_entity = MAX_ENTITIES;
|
|
}
|
|
|
|
void physics_update()
|
|
{
|
|
for (unsigned i = 0; i < max; ++i) {
|
|
if (!entries[i].valid)
|
|
continue;
|
|
physics_t *st = &entries[i].state;
|
|
|
|
st->dir = mat2_mul_mat2(mat2_rotation(st->rot), st->dir);
|
|
st->pos = vec2_add(st->pos, st->vel);
|
|
|
|
if (st->pos.y > 1)
|
|
st->pos.y -= 2;
|
|
else if (st->pos.y <= -1)
|
|
st->pos.y += 2;
|
|
|
|
if (st->pos.x >= aspect && i != escape_entity)
|
|
st->pos.x -= 2 * aspect;
|
|
else if (st->pos.x < -aspect)
|
|
st->pos.x += 2 * aspect;
|
|
}
|
|
}
|
|
|
|
void physics_add(
|
|
unsigned entity, vec2_t pos, mat2_t dir, vec2_t vel, float rot,
|
|
float mass)
|
|
{
|
|
assert(entity < MAX_ENTITIES);
|
|
if (entity >= max)
|
|
max = entity + 1;
|
|
else
|
|
assert(!entries[entity].valid);
|
|
|
|
entry_t *entry = entries + entity;
|
|
entry->valid = true;
|
|
entry->component_id
|
|
= entity_add_component(entity, update, remove, entry);
|
|
|
|
entry->state = (physics_t) {
|
|
.pos = pos,
|
|
.dir = dir,
|
|
.vel = vel,
|
|
.rot = rot,
|
|
.mass = mass,
|
|
};
|
|
}
|
|
|
|
physics_t *physics_get(unsigned entity)
|
|
{
|
|
assert(entity < max);
|
|
assert(entries[entity].valid);
|
|
return &entries[entity].state;
|
|
}
|
|
|
|
void physics_escape(unsigned entity)
|
|
{
|
|
assert(entity < max);
|
|
assert(entries[entity].valid);
|
|
escape_entity = entity;
|
|
}
|
|
|
|
static void pos_views(vec2_t a, vec2_t b, vec2_t out[4])
|
|
{
|
|
const float dx = 2 * aspect;
|
|
const float dy = 2;
|
|
out[0] = b;
|
|
out[1] = (vec2_t) { b.x + copysign(dx, a.x), b.y };
|
|
out[2] = (vec2_t) { b.x, b.y + copysign(dy, a.y) };
|
|
out[3] = (vec2_t) { b.x + copysign(dx, a.x), b.y + copysign(dy, a.y) };
|
|
}
|
|
|
|
physics_sep_t physics_separation(unsigned a, unsigned b)
|
|
{
|
|
const physics_t *pa = physics_get(a);
|
|
const physics_t *pb = physics_get(b);
|
|
|
|
vec2_t pos[4];
|
|
pos_views(pa->pos, pb->pos, pos);
|
|
|
|
float min_dist = INFINITY;
|
|
vec2_t min_disp;
|
|
for (unsigned i = 0; i < 4; ++i) {
|
|
const vec2_t disp = vec2_sub(pos[i], pa->pos);
|
|
const float dist = vec2_len(disp);
|
|
if (dist < min_dist) {
|
|
min_disp = disp;
|
|
min_dist = dist;
|
|
}
|
|
}
|
|
|
|
const vec2_t norm = vec2_norm(min_disp);
|
|
return (physics_sep_t) {
|
|
.dist = min_dist,
|
|
.norm = norm,
|
|
.va = vec2_dot(pa->vel, norm),
|
|
.vb = vec2_dot(pb->vel, norm),
|
|
};
|
|
}
|
|
|
|
void physics_bounce(unsigned a, unsigned b, physics_sep_t sep)
|
|
{
|
|
physics_t *pa = physics_get(a);
|
|
physics_t *pb = physics_get(b);
|
|
|
|
const float ma = pa->mass;
|
|
const float mb = pb->mass;
|
|
const float ms = ma + mb;
|
|
const float va = (sep.va * (ma - mb) + 2 * mb * sep.vb) / ms;
|
|
const float vb = (sep.vb * (mb - ma) + 2 * ma * sep.va) / ms;
|
|
|
|
pa->vel = vec2_add(pa->vel, vec2_scale(sep.norm, va - sep.va));
|
|
pb->vel = vec2_add(pb->vel, vec2_scale(sep.norm, vb - sep.vb));
|
|
}
|