95 lines
2.2 KiB
C
95 lines
2.2 KiB
C
|
#include "./physics.h"
|
||
|
|
|
||
|
|
#include <assert.h>
|
||
|
|
#include <stdbool.h>
|
||
|
|
#include <stddef.h>
|
||
|
|
|
||
|
|
#include "util/coly2d/hittest.h"
|
||
|
|
#include "util/math/algorithm.h"
|
||
|
|
#include "util/math/vector.h"
|
||
|
|
|
||
|
|
#include "./position.h"
|
||
|
|
|
||
|
|
bool locommon_physics_entity_valid(const locommon_physics_entity_t* e) {
|
||
|
|
return
|
||
|
|
e != NULL &&
|
||
|
|
locommon_position_valid(&e->pos) &&
|
||
|
|
vec2_valid(&e->velocity) &&
|
||
|
|
vec2_valid(&e->size);
|
||
|
|
}
|
||
|
|
|
||
|
|
bool locommon_physics_solve_collision_with_fixed_one(
|
||
|
|
locommon_physics_entity_t* e1,
|
||
|
|
const locommon_physics_entity_t* e2,
|
||
|
|
float dt) {
|
||
|
|
assert(locommon_physics_entity_valid(e1));
|
||
|
|
assert(locommon_physics_entity_valid(e2));
|
||
|
|
assert(vec2_pow_length(&e2->velocity) == 0);
|
||
|
|
assert(MATH_FLOAT_VALID(dt));
|
||
|
|
|
||
|
|
vec2_t size;
|
||
|
|
vec2_add(&size, &e1->size, &e2->size);
|
||
|
|
|
||
|
|
vec2_t pos;
|
||
|
|
locommon_position_sub(&pos, &e1->pos, &e2->pos);
|
||
|
|
pos.x /= size.x;
|
||
|
|
pos.y /= size.y;
|
||
|
|
|
||
|
|
vec2_t velocity = e1->velocity;
|
||
|
|
velocity.x /= size.x;
|
||
|
|
velocity.y /= size.y;
|
||
|
|
|
||
|
|
vec2_t disp;
|
||
|
|
vec2_mul(&disp, &velocity, dt);
|
||
|
|
|
||
|
|
vec2_t spos;
|
||
|
|
vec2_sub(&spos, &pos, &disp);
|
||
|
|
|
||
|
|
static const vec2_t origin = vec2(0, 0);
|
||
|
|
static const vec2_t sz = vec2(1, 1);
|
||
|
|
if (!coly2d_hittest_lineseg_and_rect(&spos, &pos, &origin, &sz)) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (MATH_ABS(spos.x) < 1 && MATH_ABS(spos.y) < 1) {
|
||
|
|
float* f = MATH_ABS(spos.x) > MATH_ABS(spos.y)? &spos.x: &spos.y;
|
||
|
|
*f = MATH_SIGN(*f);
|
||
|
|
}
|
||
|
|
|
||
|
|
vec2_t vt = vec2(MATH_INF, MATH_INF);
|
||
|
|
if (velocity.x != 0) {
|
||
|
|
vt.x = -(MATH_SIGN(velocity.x)+spos.x) / velocity.x;
|
||
|
|
}
|
||
|
|
if (velocity.y != 0) {
|
||
|
|
vt.y = -(MATH_SIGN(velocity.y)+spos.y) / velocity.y;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ---- simulation ---- */
|
||
|
|
float t = MATH_MIN(vt.x, vt.y);
|
||
|
|
if (t < 0) t = MATH_MAX(vt.x, vt.y);
|
||
|
|
if (t < 0 || t >= dt) return false;
|
||
|
|
|
||
|
|
vec2_t d, v = velocity, p = spos;
|
||
|
|
vec2_mul(&d, &v, t);
|
||
|
|
vec2_addeq(&p, &d);
|
||
|
|
|
||
|
|
if (t == vt.x) v.x = 0;
|
||
|
|
if (t == vt.y) v.y = 0;
|
||
|
|
vec2_mul(&d, &v, dt-t);
|
||
|
|
vec2_addeq(&p, &d);
|
||
|
|
|
||
|
|
/* ---- return result ---- */
|
||
|
|
p.x *= size.x;
|
||
|
|
p.y *= size.y;
|
||
|
|
v.x *= size.x;
|
||
|
|
v.y *= size.y;
|
||
|
|
|
||
|
|
e1->pos = e2->pos;
|
||
|
|
vec2_addeq(&e1->pos.fract, &p);
|
||
|
|
locommon_position_reduce(&e1->pos);
|
||
|
|
|
||
|
|
e1->velocity = v;
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|