#include "physics.h"
/*
this file provides a default physics implementation which can be invoked by any given "thing"
things are expected to do their own integration in their update functions, integrate_physics() is available
this callback resolves collisions for predefined types
physics things MUST get:
[
{ "pos", FIELD_VEC3 },
{ "vel", FIELD_VEC3 }, // if present object is movable, if not it is "fixed"
{ "shape", FIELD_INT }, // COLLISION_SPHERE, COLLISION_BOX,
(if shape = COLLISION_SPHERE) { "radius", FIELD_FLOAT },
(if shape = COLLISION_BOX) { "half_extents", FIELD_VEC3 },
]
*/
void integrate_physics(ThingID id) {
// helper function to add velocity to position, expects {vec3 "pos", vec3 "vel"} to be defined
if (!thing_has_field(id, "vel"))
return;
vec3 pos, vel;
thing_get_vec3(id, "pos", pos);
thing_get_vec3(id, "vel", vel);
glm_vec3_add(pos, vel, pos);
thing_set_vec3(id, "pos", pos);
}
void default_physics(ThingID id) {
// for all things after our current thing (skipping ones already checked and self), pass to resolvers
for (int other = id + 1; other < MAX_THINGS; ++other) {
int shape_a = thing_get_int(id, "shape");
int shape_b = thing_get_int(other, "shape");
if (shape_a == COLLISION_SPHERE && shape_b == COLLISION_SPHERE) {
resolve_sphere_vs_sphere(id, other);
}
else if (shape_a == COLLISION_SPHERE && shape_b == COLLISION_BOX) {
resolve_sphere_vs_box(id, other);
}
else if (shape_a == COLLISION_BOX && shape_b == COLLISION_SPHERE) {
resolve_sphere_vs_box(other, id);
}
else if (shape_a == COLLISION_BOX && shape_b == COLLISION_BOX) {
resolve_box_vs_box(id, other);
}
else {
}
}
}
void resolve_sphere_vs_sphere(ThingID a, ThingID b) {
int has_vel_a = thing_has_field(a, "vel");
int has_vel_b = thing_has_field(b, "vel");
if (!has_vel_a && !has_vel_b) return;
vec3 pos_a, pos_b;
thing_get_vec3(a, "pos", pos_a);
thing_get_vec3(b, "pos", pos_b);
float r1 = thing_get_float(a, "radius");
float r2 = thing_get_float(b, "radius");
vec3 delta;
glm_vec3_sub(pos_b, pos_a, delta);
float dist = glm_vec3_norm(delta);
float penetration = r1 + r2 - dist;
if (penetration > 0.0f && dist > 0.0001f) {
vec3 normal;
glm_vec3_scale(delta, 1.0f / dist, normal);
if (!has_vel_a && has_vel_b) {
vec3 correction;
glm_vec3_scale(normal, penetration, correction);
glm_vec3_add(pos_b, correction, pos_b);
thing_set_vec3(b, "pos", pos_b);
}
else if (has_vel_a && !has_vel_b) {
vec3 correction;
glm_vec3_scale(normal, penetration, correction);
glm_vec3_sub(pos_a, correction, pos_a);
thing_set_vec3(a, "pos", pos_a);
}
else {
vec3 correction;
glm_vec3_scale(normal, penetration * 0.5f, correction);
glm_vec3_sub(pos_a, correction, pos_a);
glm_vec3_add(pos_b, correction, pos_b);
thing_set_vec3(a, "pos", pos_a);
thing_set_vec3(b, "pos", pos_b);
}
}
}
void resolve_box_vs_box(ThingID a, ThingID b) {
int has_vel_a = thing_has_field(a, "vel");
int has_vel_b = thing_has_field(b, "vel");
if (!has_vel_a && !has_vel_b) return;
vec3 pos_a, pos_b, half_a, half_b;
thing_get_vec3(a, "pos", pos_a);
thing_get_vec3(b, "pos", pos_b);
thing_get_vec3(a, "half_extents", half_a);
thing_get_vec3(b, "half_extents", half_b);
vec3 delta;
glm_vec3_sub(pos_b, pos_a, delta);
vec3 overlap;
overlap[0] = (half_a[0] + half_b[0]) - fabsf(delta[0]);
overlap[1] = (half_a[1] + half_b[1]) - fabsf(delta[1]);
overlap[2] = (half_a[2] + half_b[2]) - fabsf(delta[2]);
if (overlap[0] > 0 && overlap[1] > 0 && overlap[2] > 0) {
int axis = 0;
if (overlap[1] < overlap[axis]) axis = 1;
if (overlap[2] < overlap[axis]) axis = 2;
float sign = (delta[axis] > 0) ? 1.0f : -1.0f;
float resolve = (has_vel_a && has_vel_b) ? overlap[axis] * 0.5f : overlap[axis];
if (!has_vel_a && has_vel_b) {
pos_b[axis] += sign * resolve;
thing_set_vec3(b, "pos", pos_b);
}
else if (has_vel_a && !has_vel_b) {
pos_a[axis] -= sign * resolve;
thing_set_vec3(a, "pos", pos_a);
}
else if (has_vel_a && has_vel_b) {
pos_a[axis] -= sign * resolve;
pos_b[axis] += sign * resolve;
thing_set_vec3(a, "pos", pos_a);
thing_set_vec3(b, "pos", pos_b);
}
if (has_vel_a) {
vec3 vel_a;
thing_get_vec3(a, "vel", vel_a);
if (sign * vel_a[axis] > 0) {
vel_a[axis] = 0;
thing_set_vec3(a, "vel", vel_a);
}
}
if (has_vel_b) {
vec3 vel_b;
thing_get_vec3(b, "vel", vel_b);
if (-sign * vel_b[axis] > 0) {
vel_b[axis] = 0;
thing_set_vec3(b, "vel", vel_b);
}
}
}
//printf("PHYSICS BOX ID %d: min=(%.2f,%.2f,%.2f) max=(%.2f,%.2f,%.2f)\n",
// a,
// pos_a[0] - half_a[0], pos_a[1] - half_a[1], pos_a[2] - half_a[2],
// pos_a[0] + half_a[0], pos_a[1] + half_a[1], pos_a[2] + half_a[2]);
}
void resolve_sphere_vs_box(ThingID a, ThingID b) {
int has_vel_a = thing_has_field(a, "vel");
int has_vel_b = thing_has_field(b, "vel");
if (!has_vel_a && !has_vel_b) return;
vec3 a_pos, b_pos, b_half;
thing_get_vec3(a, "pos", a_pos);
thing_get_vec3(b, "pos", b_pos);
thing_get_vec3(b, "half_extents", b_half);
float radius = thing_get_float(a, "radius");
vec3 closest;
for (int i = 0; i < 3; ++i) {
float min = b_pos[i] - b_half[i];
float max = b_pos[i] + b_half[i];
closest[i] = fmaxf(min, fminf(a_pos[i], max));
}
vec3 delta;
glm_vec3_sub(a_pos, closest, delta);
float dist = glm_vec3_norm(delta);
float penetration = radius - dist;
const float epsilon = 0.0001f;
if (penetration > 0.0f && dist > epsilon) {
vec3 normal;
glm_vec3_scale(delta, 1.0f / dist, normal);
if (!has_vel_a && has_vel_b) {
vec3 correction;
glm_vec3_scale(normal, penetration, correction);
glm_vec3_add(b_pos, correction, b_pos);
thing_set_vec3(b, "pos", b_pos);
}
else if (has_vel_a && !has_vel_b) {
vec3 correction;
glm_vec3_scale(normal, penetration, correction);
glm_vec3_add(a_pos, correction, a_pos);
thing_set_vec3(a, "pos", a_pos);
}
else if (has_vel_a && has_vel_b) {
vec3 correction;
glm_vec3_scale(normal, penetration * 0.5f, correction);
glm_vec3_add(a_pos, correction, a_pos);
glm_vec3_sub(b_pos, correction, b_pos);
thing_set_vec3(a, "pos", a_pos);
thing_set_vec3(b, "pos", b_pos);
}
if (has_vel_a && normal[1] > 0.5f) {
vec3 vel_a;
thing_get_vec3(a, "vel", vel_a);
if (vel_a[1] < 0.0f) {
vel_a[1] = 0.0f;
thing_set_vec3(a, "vel", vel_a);
}
}
}
}
void no_physics(ThingID id) {
}