1318 lines
53 KiB
C
1318 lines
53 KiB
C
#include "phys.h"
|
|
#include "arena.h"
|
|
#include "sim_ent.h"
|
|
#include "sim_step.h"
|
|
#include "math.h"
|
|
#include "space.h"
|
|
#include "uid.h"
|
|
|
|
#define CONTACT_SPRING_HZ 25
|
|
#define CONTACT_SPRING_DAMP 10
|
|
|
|
/* ========================== *
|
|
* Contact
|
|
* ========================== */
|
|
|
|
INTERNAL b32 can_contact(struct sim_ent *e0, struct sim_ent *e1)
|
|
{
|
|
b32 res = 0;
|
|
res = e0 != e1 &&
|
|
!sim_ent_id_eq(e0->top, e1->top) &&
|
|
!(sim_ent_has_prop(e0, SEPROP_WALL) && sim_ent_has_prop(e1, SEPROP_WALL));
|
|
return res;
|
|
}
|
|
|
|
void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt, u64 phys_iteration)
|
|
{
|
|
__prof;
|
|
struct sim_step_ctx *sim_step_ctx = ctx->sim_step_ctx;
|
|
struct sim_snapshot *ss = sim_step_ctx->world;
|
|
struct space *space = sim_step_ctx->accel->space;
|
|
struct sim_ent_id local_player = ss->local_player;
|
|
phys_collision_callback_func *collision_callback = ctx->collision_callback;
|
|
|
|
struct sim_ent *root = sim_ent_from_id(ss, SIM_ENT_ROOT_ID);
|
|
u64 tick = ss->tick;
|
|
|
|
for (u64 check0_index = 0; check0_index < ss->num_ents_reserved; ++check0_index) {
|
|
struct sim_ent *check0 = &ss->ents[check0_index];
|
|
if (!sim_ent_is_valid_and_active(check0)) continue;
|
|
if (!(sim_ent_has_prop(check0, SEPROP_SOLID) || sim_ent_has_prop(check0, SEPROP_SENSOR))) continue;
|
|
if (check0->local_collider.count <= 0) continue;
|
|
|
|
struct xform check0_xf = sim_ent_get_xform(check0);
|
|
struct collider_shape check0_collider = check0->local_collider;
|
|
struct aabb aabb = collider_aabb_from_collider(&check0_collider, check0_xf);
|
|
|
|
struct space_iter iter = space_iter_begin_aabb(space, aabb);
|
|
struct space_entry *space_entry;
|
|
while ((space_entry = space_iter_next(&iter)) != 0) {
|
|
struct sim_ent *check1 = sim_ent_from_id(ss, space_entry->ent);
|
|
if (!sim_ent_is_valid_and_active(check1)) continue;
|
|
if (!(sim_ent_has_prop(check1, SEPROP_SOLID) || sim_ent_has_prop(check1, SEPROP_SENSOR))) continue;
|
|
if (check1->local_collider.count <= 0) continue;
|
|
if (!can_contact(check0, check1)) continue;
|
|
|
|
/* Deterministic order based on entity id */
|
|
struct sim_ent *e0;
|
|
struct sim_ent *e1;
|
|
struct xform e0_xf;
|
|
struct xform e1_xf;
|
|
struct collider_shape e0_collider;
|
|
struct collider_shape e1_collider;
|
|
if (check0->id.uid.hi < check1->id.uid.hi) {
|
|
e0 = check0;
|
|
e1 = check1;
|
|
e0_xf = check0_xf;
|
|
e1_xf = sim_ent_get_xform(check1);
|
|
e0_collider = check0_collider;
|
|
e1_collider = check1->local_collider;
|
|
} else {
|
|
e0 = check1;
|
|
e1 = check0;
|
|
e0_xf = sim_ent_get_xform(check1);
|
|
e1_xf = check0_xf;
|
|
e0_collider = check1->local_collider;
|
|
e1_collider = check0_collider;
|
|
}
|
|
|
|
struct sim_ent_id constraint_id = sim_ent_contact_constraint_id_from_contacting_ids(local_player, e0->id, e1->id);
|
|
struct sim_ent *constraint_ent = sim_ent_from_id(ss, constraint_id);
|
|
|
|
if (constraint_ent->valid) {
|
|
if (constraint_ent->contact_constraint_data.last_phys_iteration >= phys_iteration) {
|
|
/* Already processed constraint this iteration */
|
|
continue;
|
|
} else {
|
|
constraint_ent->contact_constraint_data.last_phys_iteration = phys_iteration;
|
|
}
|
|
}
|
|
|
|
/* Calculate collision */
|
|
struct collider_collision_points_result collider_res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf);
|
|
|
|
/* Parts of algorithm are hard-coded to support 2 contact points */
|
|
STATIC_ASSERT(countof(constraint_ent->contact_constraint_data.points) == 2);
|
|
STATIC_ASSERT(countof(collider_res.points) == 2);
|
|
|
|
struct phys_contact_constraint *constraint = 0;
|
|
if (collider_res.num_points > 0) {
|
|
b32 is_start = 0;
|
|
if (!constraint_ent->valid) {
|
|
is_start = 1;
|
|
/* Create constraint */
|
|
constraint_ent = sim_ent_alloc_local_with_id(root, constraint_id);
|
|
constraint_ent->contact_constraint_data.e0 = e0->id;
|
|
constraint_ent->contact_constraint_data.e1 = e1->id;
|
|
/* Both entities must be solid and one must be dynamic for a solve to be necessary. */
|
|
constraint_ent->contact_constraint_data.skip_solve = !sim_ent_has_prop(e0, SEPROP_SOLID) || !sim_ent_has_prop(e1, SEPROP_SOLID) ||
|
|
!(sim_ent_has_prop(e0, SEPROP_DYNAMIC) || sim_ent_has_prop(e1, SEPROP_DYNAMIC));
|
|
sim_ent_enable_prop(constraint_ent, SEPROP_ACTIVE);
|
|
|
|
/* TODO: Should we recalculate normal as more contact points are added? */
|
|
sim_ent_enable_prop(constraint_ent, SEPROP_CONTACT_CONSTRAINT);
|
|
sim_ent_activate(constraint_ent, tick);
|
|
}
|
|
constraint = &constraint_ent->contact_constraint_data;
|
|
constraint->normal = collider_res.normal;
|
|
constraint->friction = math_sqrt(e0->friction * e1->friction);
|
|
|
|
/* Delete old contacts that are no longer present */
|
|
for (u32 i = 0; i < constraint->num_points; ++i) {
|
|
struct phys_contact_point *old = &constraint->points[i];
|
|
u32 id = old->id;
|
|
b32 found = 0;
|
|
for (u32 j = 0; j < collider_res.num_points; ++j) {
|
|
if (collider_res.points[j].id == id) {
|
|
found = 1;
|
|
break;
|
|
}
|
|
}
|
|
if (!found) {
|
|
/* Delete contact by replacing with last in array */
|
|
*old = constraint->points[--constraint->num_points];
|
|
--i;
|
|
}
|
|
}
|
|
|
|
/* Update / insert returned contacts */
|
|
for (u32 i = 0; i < collider_res.num_points; ++i) {
|
|
struct collider_collision_point *res_point = &collider_res.points[i];
|
|
struct v2 point = res_point->point;
|
|
f32 sep = res_point->separation;
|
|
u32 id = res_point->id;
|
|
struct phys_contact_point *contact = 0;
|
|
/* Match */
|
|
for (u32 j = 0; j < constraint->num_points; ++j) {
|
|
struct phys_contact_point *t = &constraint->points[j];
|
|
if (t->id == id) {
|
|
contact = t;
|
|
break;
|
|
}
|
|
}
|
|
if (!contact) {
|
|
/* Insert */
|
|
contact = &constraint->points[constraint->num_points++];
|
|
MEMZERO_STRUCT(contact);
|
|
contact->id = id;
|
|
constraint->pushout_velocity = 3.0f;
|
|
}
|
|
|
|
/* Update points & separation */
|
|
contact->vcp0 = v2_sub(point, e0_xf.og);
|
|
contact->vcp1 = v2_sub(point, e1_xf.og);
|
|
contact->starting_separation = sep;
|
|
|
|
#if DEVELOPER
|
|
contact->dbg_pt = point;
|
|
#endif
|
|
}
|
|
|
|
/* Skip solve based on collision direction */
|
|
{
|
|
struct v2 normal = collider_res.normal;
|
|
struct v2 dir0 = e0->collision_dir;
|
|
struct v2 dir1 = e1->collision_dir;
|
|
f32 threshold = 0.5;
|
|
b32 is_wrong_dir = 0;
|
|
if (!v2_is_zero(dir0)) {
|
|
is_wrong_dir = v2_dot(dir0, normal) <= threshold;
|
|
}
|
|
if (!v2_is_zero(dir1) && !is_wrong_dir) {
|
|
is_wrong_dir = v2_dot(dir1, v2_neg(normal)) <= threshold;
|
|
}
|
|
constraint->wrong_dir = is_wrong_dir;
|
|
}
|
|
|
|
/* Run collision callback */
|
|
if (collision_callback) {
|
|
struct phys_collision_data data = ZI;
|
|
data.e0 = e0->id;
|
|
data.e1 = e1->id;
|
|
data.normal = collider_res.normal;
|
|
data.is_start = is_start;
|
|
data.dt = elapsed_dt;
|
|
|
|
/* Calculate point */
|
|
struct v2 midpoint = collider_res.points[0].point;
|
|
if (collider_res.num_points > 1) {
|
|
midpoint = v2_add(midpoint, v2_mul(v2_sub(collider_res.points[1].point, midpoint), 0.5f));
|
|
}
|
|
data.point = midpoint;
|
|
|
|
/* Calculate relative velocity */
|
|
struct v2 vrel;
|
|
{
|
|
struct v2 v0 = e0->linear_velocity;
|
|
struct v2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
struct v2 vcp1 = v2_sub(midpoint, e1_xf.og);
|
|
struct v2 vcp0 = v2_sub(midpoint, e0_xf.og);
|
|
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
|
|
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
|
|
vrel = v2_sub(vel0, vel1);
|
|
}
|
|
data.vrel = vrel;
|
|
|
|
/* Collision data from e1's perspective */
|
|
struct phys_collision_data data_inverted = data;
|
|
data_inverted.e0 = data.e1;
|
|
data_inverted.e1 = data.e0;
|
|
data_inverted.normal = v2_neg(data.normal);
|
|
data_inverted.vrel = v2_neg(data.vrel);
|
|
|
|
/* Run callback twice for both e0 & e1 */
|
|
b32 skip_solve0 = collision_callback(&data, sim_step_ctx);
|
|
b32 skip_solve1 = collision_callback(&data_inverted, sim_step_ctx);
|
|
if (skip_solve0 || skip_solve1) {
|
|
constraint->skip_solve = 1;
|
|
}
|
|
}
|
|
} else if (constraint_ent->valid) {
|
|
constraint_ent->contact_constraint_data.num_points = 0;
|
|
}
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
#if COLLIDER_DEBUG && COLLIDER_DEBUG_DETAILED
|
|
{
|
|
struct sim_ent_id dbg_ent_id = sim_ent_collision_debug_id_from_ids(local_player, e0->id, e1->id);
|
|
struct sim_ent *dbg_ent = sim_ent_from_id(ss, dbg_ent_id);
|
|
if (!dbg_ent->valid) {
|
|
/* FIXME: Entity never released */
|
|
dbg_ent = sim_ent_alloc_local_with_id(root, dbg_ent_id);
|
|
sim_ent_enable_prop(dbg_ent, SEPROP_COLLISION_DEBUG);
|
|
}
|
|
|
|
struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data;
|
|
|
|
dbg->e0 = e0->id;
|
|
dbg->e1 = e1->id;
|
|
dbg->res = collider_res;
|
|
|
|
if (constraint) {
|
|
MEMCPY(dbg->points, constraint->points, sizeof(dbg->points));
|
|
dbg->num_points = constraint->num_points;
|
|
} else {
|
|
dbg->num_points = 0;
|
|
}
|
|
|
|
dbg->xf0 = e0_xf;
|
|
dbg->xf1 = e1_xf;
|
|
|
|
/* Update closest points */
|
|
{
|
|
struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf);
|
|
dbg->closest0 = closest_points_res.p0;
|
|
dbg->closest1 = closest_points_res.p1;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
space_iter_end(&iter);
|
|
}
|
|
}
|
|
|
|
void phys_prepare_contacts(struct phys_step_ctx *ctx, u64 phys_iteration)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(constraint_ent)) continue;
|
|
if (!sim_ent_has_prop(constraint_ent, SEPROP_CONTACT_CONSTRAINT)) continue;
|
|
|
|
struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, constraint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, constraint->e1);
|
|
if (constraint->last_phys_iteration >= phys_iteration && num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1)) {
|
|
struct v2 normal = constraint->normal;
|
|
struct v2 tangent = v2_perp(normal);
|
|
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m0 = 0;
|
|
f32 inv_m1 = 0;
|
|
f32 inv_i0 = 0;
|
|
f32 inv_i1 = 0;
|
|
{
|
|
/* If not simulated locally or ent is not dynamic, pretend contact mass is infinite */
|
|
if (sim_ent_should_simulate(e0) && sim_ent_has_prop(e0, SEPROP_DYNAMIC)) {
|
|
f32 scale = math_fabs(xform_get_determinant(e0_xf));
|
|
f32 scaled_mass = e0->mass_unscaled * scale;
|
|
f32 scaled_inertia = e0->inertia_unscaled * scale;
|
|
inv_m0 = 1.f / scaled_mass;
|
|
inv_i0 = 1.f / scaled_inertia;
|
|
}
|
|
if (sim_ent_should_simulate(e1) && sim_ent_has_prop(e1, SEPROP_DYNAMIC)) {
|
|
f32 scale = math_fabs(xform_get_determinant(e1_xf));
|
|
f32 scaled_mass = e1->mass_unscaled * scale;
|
|
f32 scaled_inertia = e1->inertia_unscaled * scale;
|
|
inv_m1 = 1.f / scaled_mass;
|
|
inv_i1 = 1.f / scaled_inertia;
|
|
}
|
|
|
|
}
|
|
constraint->inv_m0 = inv_m0;
|
|
constraint->inv_m1 = inv_m1;
|
|
constraint->inv_i0 = inv_i0;
|
|
constraint->inv_i1 = inv_i1;
|
|
|
|
/* Update / insert returned contacts */
|
|
for (u32 i = 0; i < num_points; ++i) {
|
|
struct phys_contact_point *contact = &constraint->points[i];
|
|
struct v2 vcp0 = contact->vcp0;
|
|
struct v2 vcp1 = contact->vcp1;
|
|
|
|
/* Normal mass */
|
|
{
|
|
f32 vcp0_wedge = v2_wedge(vcp0, normal);
|
|
f32 vcp1_wedge = v2_wedge(vcp1, normal);
|
|
f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge);
|
|
contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f;
|
|
}
|
|
|
|
/* Tangent mass */
|
|
{
|
|
f32 vcp0_wedge = v2_wedge(vcp0, tangent);
|
|
f32 vcp1_wedge = v2_wedge(vcp1, tangent);
|
|
f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge);
|
|
contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f;
|
|
}
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
contact->normal_impulse = 0;
|
|
contact->tangent_impulse = 0;
|
|
#endif
|
|
}
|
|
} else {
|
|
/* Mark constraint for removal */
|
|
constraint_ent->contact_constraint_data.num_points = 0;
|
|
sim_ent_disable_prop(constraint_ent, SEPROP_ACTIVE);
|
|
sim_ent_enable_prop(constraint_ent, SEPROP_RELEASE);
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
#if COLLIDER_DEBUG
|
|
/* Remove collision debug ents */
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *dbg_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(dbg_ent)) continue;
|
|
if (!sim_ent_has_prop(dbg_ent, SEPROP_COLLISION_DEBUG)) continue;
|
|
|
|
struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data;
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, dbg->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, dbg->e1);
|
|
|
|
if (!(sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) ||
|
|
!(sim_ent_has_prop(e0, SEPROP_SOLID) || sim_ent_has_prop(e0, SEPROP_SENSOR)) ||
|
|
!(sim_ent_has_prop(e1, SEPROP_SOLID) || sim_ent_has_prop(e1, SEPROP_SENSOR))) {
|
|
/* Mark dbg ent for removal */
|
|
sim_ent_disable_prop(dbg_ent, SEPROP_ACTIVE);
|
|
sim_ent_enable_prop(dbg_ent, SEPROP_RELEASE);
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
void phys_warm_start_contacts(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(constraint_ent)) continue;
|
|
if (!sim_ent_has_prop(constraint_ent, SEPROP_CONTACT_CONSTRAINT)) continue;
|
|
|
|
struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, constraint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, constraint->e1);
|
|
|
|
if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve && !constraint->wrong_dir) {
|
|
f32 inv_m0 = constraint->inv_m0;
|
|
f32 inv_m1 = constraint->inv_m1;
|
|
f32 inv_i0 = constraint->inv_i0;
|
|
f32 inv_i1 = constraint->inv_i1;
|
|
|
|
struct v2 v0 = e0->linear_velocity;
|
|
struct v2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
/* Warm start */
|
|
struct v2 normal = constraint->normal;
|
|
struct v2 tangent = v2_perp(normal);
|
|
f32 inv_num_points = 1.f / num_points;
|
|
for (u32 i = 0; i < num_points; ++i) {
|
|
struct phys_contact_point *point = &constraint->points[i];
|
|
struct v2 vcp0 = point->vcp0;
|
|
struct v2 vcp1 = point->vcp1;
|
|
|
|
struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse));
|
|
impulse = v2_mul(impulse, inv_num_points);
|
|
|
|
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
|
|
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
|
|
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
|
|
w1 += v2_wedge(vcp1, impulse) * inv_i1;
|
|
}
|
|
|
|
sim_ent_set_linear_velocity(e0, v0);
|
|
sim_ent_set_angular_velocity(e0, w0);
|
|
sim_ent_set_linear_velocity(e1, v1);
|
|
sim_ent_set_angular_velocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
void phys_solve_contacts(struct phys_step_ctx *ctx, f32 dt, b32 apply_bias)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(constraint_ent)) continue;
|
|
if (!sim_ent_has_prop(constraint_ent, SEPROP_CONTACT_CONSTRAINT)) continue;
|
|
|
|
struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, constraint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, constraint->e1);
|
|
|
|
struct v2 v0 = e0->linear_velocity;
|
|
struct v2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve && !constraint->wrong_dir) {
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
f32 inv_m0 = constraint->inv_m0;
|
|
f32 inv_m1 = constraint->inv_m1;
|
|
f32 inv_i0 = constraint->inv_i0;
|
|
f32 inv_i1 = constraint->inv_i1;
|
|
|
|
/* Normal impulse */
|
|
struct v2 normal = constraint->normal;
|
|
for (u32 point_index = 0; point_index < num_points; ++point_index) {
|
|
struct phys_contact_point *point = &constraint->points[point_index];
|
|
struct v2 vcp0 = point->vcp0;
|
|
struct v2 vcp1 = point->vcp1;
|
|
struct v2 p0 = v2_add(e0_xf.og, vcp0);
|
|
struct v2 p1 = v2_add(e1_xf.og, vcp1);
|
|
|
|
/* FIXME: Should separation use the rotated contact points? */
|
|
f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation;
|
|
|
|
f32 velocity_bias = 0.0f;
|
|
f32 mass_scale = 1.0f;
|
|
f32 impulse_scale = 0.0f;
|
|
|
|
if (separation > 0.0f) {
|
|
/* Speculative */
|
|
velocity_bias = separation / dt;
|
|
} else if (apply_bias) {
|
|
/* Soft constraint */
|
|
struct math_spring softness = math_spring_init(CONTACT_SPRING_HZ, CONTACT_SPRING_DAMP, dt);
|
|
f32 pushout_velocity = constraint->pushout_velocity;
|
|
mass_scale = softness.mass_scale;
|
|
impulse_scale = softness.impulse_scale;
|
|
velocity_bias = max_f32(softness.bias_rate * separation, -pushout_velocity);
|
|
}
|
|
|
|
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
|
|
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
|
|
struct v2 vrel = v2_sub(vel0, vel1);
|
|
|
|
f32 k = point->inv_normal_mass;
|
|
|
|
/* (to be applied along n) */
|
|
f32 vn = v2_dot(vrel, normal);
|
|
f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale);
|
|
|
|
f32 old_impulse = point->normal_impulse;
|
|
f32 new_impulse = max_f32(old_impulse + j, 0);
|
|
f32 delta = new_impulse - old_impulse;
|
|
point->normal_impulse = new_impulse;
|
|
|
|
struct v2 impulse = v2_mul(normal, delta);
|
|
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
|
|
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
|
|
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
|
|
w1 += v2_wedge(vcp1, impulse) * inv_i1;
|
|
}
|
|
|
|
/* Tangent impulse */
|
|
struct v2 tangent = v2_perp(normal);
|
|
for (u32 point_index = 0; point_index < num_points; ++point_index) {
|
|
struct phys_contact_point *point = &constraint->points[point_index];
|
|
struct v2 vcp0 = point->vcp0;
|
|
struct v2 vcp1 = point->vcp1;
|
|
|
|
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
|
|
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
|
|
struct v2 vrel = v2_sub(vel0, vel1);
|
|
|
|
f32 k = point->inv_tangent_mass;
|
|
|
|
/* (to be applied along t) */
|
|
f32 vt = v2_dot(vrel, tangent);
|
|
f32 j = vt * k;
|
|
|
|
f32 max_friction = constraint->friction * point->normal_impulse;
|
|
f32 old_impulse = point->tangent_impulse;
|
|
f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction);
|
|
f32 delta = new_impulse - old_impulse;
|
|
point->tangent_impulse = new_impulse;
|
|
|
|
struct v2 impulse = v2_mul(tangent, delta);
|
|
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
|
|
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
|
|
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
|
|
w1 += v2_wedge(vcp1, impulse) * inv_i1;
|
|
}
|
|
|
|
sim_ent_set_linear_velocity(e0, v0);
|
|
sim_ent_set_angular_velocity(e0, w0);
|
|
sim_ent_set_linear_velocity(e1, v1);
|
|
sim_ent_set_angular_velocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Motor joint
|
|
* ========================== */
|
|
|
|
struct phys_motor_joint_def phys_motor_joint_def_init(void)
|
|
{
|
|
struct phys_motor_joint_def def = ZI;
|
|
return def;
|
|
}
|
|
|
|
struct phys_motor_joint phys_motor_joint_from_def(struct phys_motor_joint_def def)
|
|
{
|
|
struct phys_motor_joint res = ZI;
|
|
res.e0 = def.e0;
|
|
res.e1 = def.e1;
|
|
res.correction_rate = clamp_f32(def.correction_rate, 0, 1);
|
|
res.max_force = def.max_force;
|
|
res.max_torque = def.max_torque;
|
|
return res;
|
|
}
|
|
|
|
void phys_prepare_motor_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOTOR_JOINT)) continue;
|
|
|
|
struct phys_motor_joint *joint = &joint_ent->motor_joint_data;
|
|
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
|
|
if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) {
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m0;
|
|
f32 inv_m1;
|
|
f32 inv_i0;
|
|
f32 inv_i1;
|
|
{
|
|
f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
|
|
f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
|
|
inv_m0 = 1.f / (e0->mass_unscaled * scale0);
|
|
inv_m1 = 1.f / (e1->mass_unscaled * scale1);
|
|
inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
|
|
inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
|
|
}
|
|
joint->inv_m0 = inv_m0;
|
|
joint->inv_m1 = inv_m1;
|
|
joint->inv_i0 = inv_i0;
|
|
joint->inv_i1 = inv_i1;
|
|
|
|
joint->point_local_e0 = V2(0, 0);
|
|
joint->point_local_e1 = V2(0, 0);
|
|
|
|
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
struct xform linear_mass_xf = ZI;
|
|
linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1;
|
|
linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1;
|
|
linear_mass_xf.by.x = linear_mass_xf.bx.y;
|
|
linear_mass_xf.by.y = inv_m0 + inv_m1 + vcp0.x * vcp0.x * inv_i0 + vcp1.x * vcp1.x * inv_i1;
|
|
joint->linear_mass_xf = xform_invert(linear_mass_xf);
|
|
|
|
joint->angular_mass = 1.f / (inv_i0 + inv_i1);
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse = V2(0, 0);
|
|
joint->angular_impulse = 0;
|
|
#endif
|
|
} else {
|
|
/* Mark joint for removal */
|
|
sim_ent_disable_prop(joint_ent, SEPROP_ACTIVE);
|
|
sim_ent_enable_prop(joint_ent, SEPROP_RELEASE);
|
|
}
|
|
}
|
|
}
|
|
|
|
void phys_warm_start_motor_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOTOR_JOINT)) continue;
|
|
|
|
struct phys_motor_joint *joint = &joint_ent->motor_joint_data;
|
|
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
f32 inv_m0 = joint->inv_m0;
|
|
f32 inv_m1 = joint->inv_m1;
|
|
f32 inv_i0 = joint->inv_i0;
|
|
f32 inv_i1 = joint->inv_i1;
|
|
|
|
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
sim_ent_set_linear_velocity(e0, v2_sub(e0->linear_velocity, v2_mul(joint->linear_impulse, inv_m0)));
|
|
sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse, inv_m1)));
|
|
e0->angular_velocity -= (v2_wedge(vcp0, joint->linear_impulse) + joint->angular_impulse) * inv_i0;
|
|
e1->angular_velocity += (v2_wedge(vcp1, joint->linear_impulse) + joint->angular_impulse) * inv_i1;
|
|
}
|
|
}
|
|
|
|
void phys_solve_motor_joints(struct phys_step_ctx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOTOR_JOINT)) continue;
|
|
|
|
struct phys_motor_joint *joint = &joint_ent->motor_joint_data;
|
|
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
f32 inv_m0 = joint->inv_m0;
|
|
f32 inv_m1 = joint->inv_m1;
|
|
f32 inv_i0 = joint->inv_i0;
|
|
f32 inv_i1 = joint->inv_i1;
|
|
|
|
struct v2 v0 = e0->linear_velocity;
|
|
struct v2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
f32 correction_rate = joint->correction_rate;
|
|
f32 inv_dt = 1.f / dt;
|
|
|
|
/* Angular constraint */
|
|
{
|
|
f32 max_impulse = joint->max_torque * dt;
|
|
f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf));
|
|
|
|
f32 angular_bias = angular_separation * correction_rate * inv_dt;
|
|
|
|
f32 old_impulse = joint->angular_impulse;
|
|
f32 new_impulse = clamp_f32(old_impulse + (-joint->angular_mass * (w1 - w0 + angular_bias)), -max_impulse, max_impulse);
|
|
joint->angular_impulse = new_impulse;
|
|
|
|
f32 delta = new_impulse - old_impulse;
|
|
w0 -= delta * inv_i0;
|
|
w1 += delta * inv_i1;
|
|
}
|
|
|
|
/* Linear constraint */
|
|
{
|
|
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
f32 max_impulse = joint->max_force * dt;
|
|
|
|
struct v2 linear_separation = v2_sub(v2_add(e1_xf.og, vcp1), v2_add(e0_xf.og, vcp0));
|
|
struct v2 linear_bias = v2_mul(linear_separation, correction_rate * inv_dt);
|
|
|
|
struct v2 vrel = v2_sub(v2_add(v1, v2_perp_mul(vcp1, w1)), v2_add(v0, v2_perp_mul(vcp0, w0)));
|
|
struct v2 impulse = v2_neg(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vrel, linear_bias)));
|
|
|
|
struct v2 old_impulse = joint->linear_impulse;
|
|
struct v2 new_impulse = v2_clamp_len(v2_add(old_impulse, impulse), max_impulse);
|
|
joint->linear_impulse = new_impulse;
|
|
|
|
struct v2 delta = v2_sub(new_impulse, old_impulse);
|
|
v0 = v2_sub(v0, v2_mul(delta, inv_m0));
|
|
v1 = v2_add(v1, v2_mul(delta, inv_m1));
|
|
w0 -= v2_wedge(vcp0, delta) * inv_i0;
|
|
w1 += v2_wedge(vcp1, delta) * inv_i1;
|
|
}
|
|
|
|
sim_ent_set_linear_velocity(e0, v0);
|
|
sim_ent_set_angular_velocity(e0, w0);
|
|
sim_ent_set_linear_velocity(e1, v1);
|
|
sim_ent_set_angular_velocity(e1, w1);
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Mouse joint
|
|
* ========================== */
|
|
|
|
struct phys_mouse_joint_def phys_mouse_joint_def_init(void)
|
|
{
|
|
struct phys_mouse_joint_def def = ZI;
|
|
def.linear_spring_hz = 5.0f;
|
|
def.linear_spring_damp = 0.7f;
|
|
def.angular_spring_hz = 5.0f;
|
|
def.angular_spring_damp = 0.1f;
|
|
def.max_force = 1000.0f;
|
|
return def;
|
|
}
|
|
|
|
struct phys_mouse_joint phys_mouse_joint_from_def(struct phys_mouse_joint_def def)
|
|
{
|
|
struct phys_mouse_joint res = ZI;
|
|
res.target = def.target;
|
|
res.point_local_start = def.point_local_start;
|
|
res.point_end = def.point_end;
|
|
res.linear_spring_hz = def.linear_spring_hz;
|
|
res.linear_spring_damp = def.linear_spring_damp;
|
|
res.angular_spring_hz = def.angular_spring_hz;
|
|
res.angular_spring_damp = def.angular_spring_damp;
|
|
res.max_force = def.max_force;
|
|
return res;
|
|
}
|
|
|
|
void phys_prepare_mouse_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOUSE_JOINT)) continue;
|
|
|
|
struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
|
|
struct sim_ent *ent = sim_ent_from_id(ss, joint->target);
|
|
if (sim_ent_should_simulate(ent)) {
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m;
|
|
f32 inv_i;
|
|
{
|
|
f32 scale = math_fabs(xform_get_determinant(xf));
|
|
inv_m = 1.f / (ent->mass_unscaled * scale);
|
|
inv_i = 1.f / (ent->inertia_unscaled * scale);
|
|
}
|
|
joint->inv_m = inv_m;
|
|
joint->inv_i = inv_i;
|
|
|
|
struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
|
|
|
|
struct xform linear_mass_xf = ZI;
|
|
linear_mass_xf.bx.x = inv_m + inv_i * vcp.y * vcp.y;
|
|
linear_mass_xf.bx.y = -inv_i * vcp.x * vcp.y;
|
|
linear_mass_xf.by.x = linear_mass_xf.bx.y;
|
|
linear_mass_xf.by.y = inv_m + inv_i * vcp.x * vcp.x;
|
|
joint->linear_mass_xf = xform_invert(linear_mass_xf);
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse = V2(0, 0);
|
|
joint->angular_impulse = 0;
|
|
#endif
|
|
} else {
|
|
/* Mark joint for removal */
|
|
sim_ent_disable_prop(joint_ent, SEPROP_ACTIVE);
|
|
sim_ent_enable_prop(joint_ent, SEPROP_RELEASE);
|
|
}
|
|
}
|
|
}
|
|
|
|
void phys_warm_start_mouse_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOUSE_JOINT)) continue;
|
|
|
|
struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
|
|
struct sim_ent *ent = sim_ent_from_id(ss, joint->target);
|
|
if (sim_ent_should_simulate(ent)) {
|
|
f32 inv_m = joint->inv_m;
|
|
f32 inv_i = joint->inv_i;
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
|
|
sim_ent_set_linear_velocity(ent, v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)));
|
|
sim_ent_set_angular_velocity(ent, ent->angular_velocity + ((v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i));
|
|
}
|
|
}
|
|
}
|
|
|
|
void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_MOUSE_JOINT)) continue;
|
|
|
|
struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
|
|
struct sim_ent *ent = sim_ent_from_id(ss, joint->target);
|
|
if (sim_ent_should_simulate(ent)) {
|
|
struct v2 v = ent->linear_velocity;
|
|
f32 w = ent->angular_velocity;
|
|
|
|
f32 inv_m = joint->inv_m;
|
|
f32 inv_i = joint->inv_i;
|
|
|
|
/* Angular impulse */
|
|
{
|
|
struct math_spring softness = math_spring_init(joint->angular_spring_hz, joint->angular_spring_damp, dt);
|
|
f32 mass_scale = softness.mass_scale;
|
|
f32 impulse_scale = softness.impulse_scale;
|
|
f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse;
|
|
joint->angular_impulse += impulse;
|
|
w += impulse * inv_i;
|
|
}
|
|
|
|
/* Linear impulse */
|
|
{
|
|
f32 max_impulse = joint->max_force / dt;
|
|
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
|
|
struct v2 point_start = xform_mul_v2(xf, joint->point_local_start);
|
|
struct v2 point_end = joint->point_end;
|
|
|
|
struct v2 vcp = v2_sub(point_start, xf.og);
|
|
struct v2 separation = v2_sub(point_start, point_end);
|
|
|
|
struct math_spring softness = math_spring_init(joint->linear_spring_hz, joint->linear_spring_damp, dt);
|
|
f32 bias_rate = softness.bias_rate;
|
|
f32 mass_scale = softness.mass_scale;
|
|
f32 impulse_scale = softness.impulse_scale;
|
|
|
|
struct v2 bias = v2_mul(separation, bias_rate);
|
|
struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
|
|
struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias));
|
|
|
|
struct v2 impulse = v2_mul(b, -mass_scale);
|
|
impulse = v2_sub(impulse, v2_mul(joint->linear_impulse, impulse_scale));
|
|
|
|
struct v2 old_impulse = joint->linear_impulse;
|
|
joint->linear_impulse = v2_add(joint->linear_impulse, impulse);
|
|
|
|
joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse);
|
|
|
|
impulse = v2_sub(joint->linear_impulse, old_impulse);
|
|
|
|
v = v2_add(v, v2_mul(impulse, inv_m));
|
|
w += v2_wedge(vcp, impulse) * inv_i;
|
|
}
|
|
|
|
sim_ent_set_linear_velocity(ent, v);
|
|
sim_ent_set_angular_velocity(ent, w);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Weld joint
|
|
* ========================== */
|
|
|
|
struct phys_weld_joint_def phys_weld_joint_def_init(void)
|
|
{
|
|
struct phys_weld_joint_def def = ZI;
|
|
def.linear_spring_hz = 50;
|
|
def.linear_spring_damp = 0;
|
|
def.angular_spring_hz = 50;
|
|
def.angular_spring_damp = 0;
|
|
return def;
|
|
}
|
|
|
|
struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def)
|
|
{
|
|
struct phys_weld_joint res = ZI;
|
|
res.e0 = def.e0;
|
|
res.e1 = def.e1;
|
|
res.linear_spring_hz = def.linear_spring_hz;
|
|
res.linear_spring_damp = def.linear_spring_damp;
|
|
res.angular_spring_hz = def.angular_spring_hz;
|
|
res.angular_spring_damp = def.angular_spring_damp;
|
|
res.xf0_to_xf1 = def.xf;
|
|
return res;
|
|
}
|
|
|
|
void phys_prepare_weld_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
|
|
|
|
/* TODO: Lookup and disable collision for any contacts between e0 & e1? */
|
|
|
|
struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) {
|
|
struct xform e0_xf = sim_ent_get_xform(e0);
|
|
struct xform e1_xf = sim_ent_get_xform(e1);
|
|
|
|
f32 inv_m0;
|
|
f32 inv_m1;
|
|
f32 inv_i0;
|
|
f32 inv_i1;
|
|
{
|
|
f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
|
|
f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
|
|
inv_m0 = 1.f / (e0->mass_unscaled * scale0);
|
|
inv_m1 = 1.f / (e1->mass_unscaled * scale1);
|
|
inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
|
|
inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
|
|
}
|
|
joint->inv_m0 = inv_m0;
|
|
joint->inv_m1 = inv_m1;
|
|
joint->inv_i0 = inv_i0;
|
|
joint->inv_i1 = inv_i1;
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse0 = V2(0, 0);
|
|
joint->linear_impulse1 = V2(0, 0);
|
|
joint->angular_impulse0 = 0;
|
|
joint->angular_impulse1 = 0;
|
|
#endif
|
|
} else {
|
|
/* Mark joint for removal */
|
|
sim_ent_disable_prop(joint_ent, SEPROP_ACTIVE);
|
|
sim_ent_enable_prop(joint_ent, SEPROP_RELEASE);
|
|
}
|
|
}
|
|
}
|
|
|
|
void phys_warm_start_weld_joints(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
|
|
|
|
struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
|
|
|
|
#if 0
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
if (sim_ent_should_simulate(e0)) {
|
|
f32 inv_m = joint->inv_m0;
|
|
f32 inv_i = joint->inv_i0;
|
|
struct xform xf = sim_ent_get_xform(e1);
|
|
struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
|
|
sim_ent_set_linear_velocity(ent, v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)));
|
|
ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i;
|
|
}
|
|
#endif
|
|
|
|
#if 1
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
if (sim_ent_should_simulate(e1)) {
|
|
f32 inv_m = joint->inv_m1;
|
|
f32 inv_i = joint->inv_i1;
|
|
sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse1, inv_m)));
|
|
sim_ent_set_angular_velocity(e1, e1->angular_velocity + joint->angular_impulse1 * inv_i);
|
|
}
|
|
#else
|
|
(UNUSED)joint;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(joint_ent)) continue;
|
|
if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
|
|
|
|
struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
|
|
struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
|
|
if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) {
|
|
struct xform xf0 = sim_ent_get_xform(e0);
|
|
struct xform xf1 = sim_ent_get_xform(e1);
|
|
|
|
struct xform target_xf1 = xform_mul(xf0, joint->xf0_to_xf1);
|
|
|
|
struct v2 v1 = e1->linear_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
/* Angular constraint */
|
|
{
|
|
struct math_spring softness = math_spring_init(joint->angular_spring_hz, joint->angular_spring_damp, dt);
|
|
f32 inv_i1 = joint->inv_i1;
|
|
f32 k = 1 / inv_i1;
|
|
f32 separation = math_unwind_angle(xform_get_rotation(target_xf1) - xform_get_rotation(xf1));
|
|
f32 bias = -separation * softness.bias_rate;
|
|
f32 b = (w1 + bias) * k;
|
|
f32 impulse = -softness.mass_scale * b - joint->angular_impulse1 * softness.impulse_scale;
|
|
joint->angular_impulse1 += impulse;
|
|
w1 += impulse * inv_i1;
|
|
}
|
|
|
|
/* Linear constraint */
|
|
{
|
|
struct math_spring softness = math_spring_init(joint->linear_spring_hz, joint->linear_spring_damp, dt);
|
|
f32 inv_m1 = joint->inv_m1;
|
|
|
|
struct v2 separation = v2_sub(xf1.og, target_xf1.og);
|
|
|
|
f32 k = 1 / inv_m1;
|
|
|
|
struct v2 bias = v2_mul(separation, softness.bias_rate);
|
|
struct v2 b = v2_mul(v2_add(v1, bias), k);
|
|
|
|
struct v2 impulse = v2_mul(b, -softness.mass_scale);
|
|
impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, softness.impulse_scale));
|
|
|
|
struct v2 old_impulse = joint->linear_impulse1;
|
|
joint->linear_impulse1 = v2_add(joint->linear_impulse1, impulse);
|
|
|
|
impulse = v2_sub(joint->linear_impulse1, old_impulse);
|
|
|
|
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
|
|
}
|
|
|
|
|
|
sim_ent_set_linear_velocity(e1, v1);
|
|
sim_ent_set_angular_velocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Integration
|
|
* ========================== */
|
|
|
|
INTERNAL struct xform get_derived_xform(struct sim_ent *ent, f32 dt)
|
|
{
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
|
|
struct v2 step_linear_velocity = v2_mul(ent->linear_velocity, dt);
|
|
f32 step_angular_velocity = ent->angular_velocity * dt;
|
|
|
|
xf.og = v2_add(xf.og, step_linear_velocity);
|
|
xf = xform_basis_rotated_world(xf, step_angular_velocity);
|
|
return xf;
|
|
}
|
|
|
|
void phys_integrate_forces(struct phys_step_ctx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(ent)) continue;
|
|
|
|
b32 is_dynamic = sim_ent_has_prop(ent, SEPROP_DYNAMIC);
|
|
b32 is_kinematic = sim_ent_has_prop(ent, SEPROP_KINEMATIC);
|
|
if (is_dynamic || is_kinematic) {
|
|
struct v2 linear_velocity = ent->linear_velocity;
|
|
f32 angular_velocity = ent->angular_velocity;
|
|
f32 linear_damping_factor = max_f32(1.0f - (ent->linear_damping * dt), 0);
|
|
f32 angular_damping_factor = max_f32(1.0f - (ent->angular_damping * dt), 0);
|
|
|
|
/* Integrate forces */
|
|
if (is_dynamic) {
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
f32 det_abs = math_fabs(xform_get_determinant(xf));
|
|
f32 mass = ent->mass_unscaled * det_abs;
|
|
f32 inertia = ent->inertia_unscaled * det_abs;
|
|
struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
|
|
f32 torque_accel = (ent->torque / inertia) * dt;
|
|
linear_velocity = v2_add(linear_velocity, force_accel);
|
|
angular_velocity += torque_accel;
|
|
}
|
|
|
|
/* Apply damping */
|
|
linear_velocity = v2_mul(linear_velocity, linear_damping_factor);
|
|
angular_velocity *= angular_damping_factor;
|
|
|
|
/* Update entity */
|
|
sim_ent_set_linear_velocity(ent, linear_velocity);
|
|
sim_ent_set_angular_velocity(ent, angular_velocity);
|
|
ent->force = V2(0, 0);
|
|
ent->torque = 0;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void phys_integrate_velocities(struct phys_step_ctx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_should_simulate(ent)) continue;
|
|
if (!sim_ent_has_prop(ent, SEPROP_DYNAMIC) && !sim_ent_has_prop(ent, SEPROP_KINEMATIC)) continue;
|
|
|
|
struct xform xf = get_derived_xform(ent, dt);
|
|
sim_ent_set_xform(ent, xf);
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Earliest time of impact
|
|
* ========================== */
|
|
|
|
f32 phys_determine_earliest_toi(struct phys_step_ctx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
struct space *space = ctx->sim_step_ctx->accel->space;
|
|
f32 smallest_t = 1;
|
|
|
|
for (u64 e0_index = 0; e0_index < ss->num_ents_reserved; ++e0_index) {
|
|
struct sim_ent *e0 = &ss->ents[e0_index];
|
|
if (!sim_ent_should_simulate(e0)) continue;
|
|
if (!(sim_ent_has_prop(e0, SEPROP_SOLID) || sim_ent_has_prop(e0, SEPROP_SENSOR))) continue;
|
|
if (!sim_ent_has_prop(e0, SEPROP_TOI)) continue;
|
|
if (e0->local_collider.count <= 0) continue;
|
|
|
|
struct collider_shape e0_collider = e0->local_collider;
|
|
struct xform e0_xf_t0 = sim_ent_get_xform(e0);
|
|
struct xform e0_xf_t1 = get_derived_xform(e0, step_dt);
|
|
|
|
/* TODO: Use swept aabb rather than combined aabb. This should prevent spikes from bullets returning 0 positive TOIs with irrelevant entities. */
|
|
struct aabb aabb_t0 = collider_aabb_from_collider(&e0_collider, e0_xf_t0);
|
|
struct aabb aabb_t1 = collider_aabb_from_collider(&e0_collider, e0_xf_t1);
|
|
struct aabb combined_aabb = collider_aabb_from_combined_aabb(aabb_t0, aabb_t1);
|
|
|
|
struct space_iter iter = space_iter_begin_aabb(space, combined_aabb);
|
|
struct space_entry *entry;
|
|
while ((entry = space_iter_next(&iter)) != 0) {
|
|
struct sim_ent *e1 = sim_ent_from_id(ss, entry->ent);
|
|
if (!sim_ent_should_simulate(e1)) continue;
|
|
if (!(sim_ent_has_prop(e1, SEPROP_SOLID) || sim_ent_has_prop(e1, SEPROP_SENSOR))) continue;
|
|
if (e1->local_collider.count <= 0) continue;
|
|
if (!can_contact(e0, e1)) continue;
|
|
|
|
struct collider_shape e1_collider = e1->local_collider;
|
|
struct xform e1_xf_t0 = sim_ent_get_xform(e1);
|
|
struct xform e1_xf_t1 = get_derived_xform(e1, step_dt);
|
|
|
|
f32 t = collider_time_of_impact(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations);
|
|
if (t != 0 && t < smallest_t) {
|
|
smallest_t = t;
|
|
}
|
|
}
|
|
space_iter_end(&iter);
|
|
}
|
|
|
|
return smallest_t;
|
|
}
|
|
|
|
/* ========================== *
|
|
* Space
|
|
* ========================== */
|
|
|
|
void phys_update_aabbs(struct phys_step_ctx *ctx)
|
|
{
|
|
__prof;
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
struct space *space = ctx->sim_step_ctx->accel->space;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
|
|
struct sim_ent *ent = &ss->ents[sim_ent_index];
|
|
if (!sim_ent_is_valid_and_active(ent)) continue;
|
|
if (ent->local_collider.count > 0) {
|
|
struct xform xf = sim_ent_get_xform(ent);
|
|
struct space_entry *space_entry = space_entry_from_handle(space, ent->space_handle);
|
|
if (!space_entry->valid) {
|
|
space_entry = space_entry_alloc(space, ent->id);
|
|
ent->space_handle = space_entry->handle;
|
|
}
|
|
struct aabb aabb = collider_aabb_from_collider(&ent->local_collider, xf);
|
|
space_entry_update_aabb(space_entry, aabb);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ========================== *
|
|
* Step
|
|
* ========================== */
|
|
|
|
/* Returns phys iteration to be fed into next step. Supplied iteration must be > 0. */
|
|
void phys_step(struct phys_step_ctx *ctx, f32 timestep)
|
|
{
|
|
__prof;
|
|
phys_integrate_forces(ctx, timestep);
|
|
struct sim_snapshot *ss = ctx->sim_step_ctx->world;
|
|
u64 phys_iteration = ss->phys_iteration;
|
|
|
|
phys_update_aabbs(ctx);
|
|
|
|
f32 remaining_dt = timestep;
|
|
while (remaining_dt > 0) {
|
|
__profn("Step part");
|
|
++phys_iteration;
|
|
struct arena_temp scratch = scratch_begin_no_conflict();
|
|
|
|
/* TOI */
|
|
f32 step_dt = remaining_dt;
|
|
{
|
|
#if SIM_PHYSICS_ENABLE_TOI
|
|
const f32 min_toi = 0.000001f;
|
|
const f32 tolerance = 0.0001f;
|
|
const u32 max_iterations = 16;
|
|
f32 earliest_toi = max_f32(phys_determine_earliest_toi(ctx, step_dt, tolerance, max_iterations), min_toi);
|
|
step_dt = remaining_dt * earliest_toi;
|
|
#else
|
|
(UNUSED)phys_determine_earliest_toi;
|
|
#endif
|
|
}
|
|
remaining_dt -= step_dt;
|
|
|
|
phys_create_and_update_contacts(ctx, timestep - remaining_dt, phys_iteration);
|
|
|
|
phys_prepare_contacts(ctx, phys_iteration);
|
|
phys_prepare_motor_joints(ctx);
|
|
phys_prepare_mouse_joints(ctx);
|
|
phys_prepare_weld_joints(ctx);
|
|
|
|
f32 substep_dt = step_dt / SIM_PHYSICS_SUBSTEPS;
|
|
for (u32 i = 0; i < SIM_PHYSICS_SUBSTEPS; ++i) {
|
|
__profn("Substep");
|
|
|
|
/* Warm start */
|
|
#if SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
phys_warm_start_contacts(ctx);
|
|
phys_warm_start_motor_joints(ctx);
|
|
phys_warm_start_mouse_joints(ctx);
|
|
phys_warm_start_weld_joints(ctx);
|
|
#endif
|
|
|
|
/* Solve */
|
|
#if SIM_PHYSICS_ENABLE_COLLISION
|
|
phys_solve_contacts(ctx, substep_dt, 1);
|
|
#endif
|
|
phys_solve_motor_joints(ctx, substep_dt);
|
|
phys_solve_mouse_joints(ctx, substep_dt);
|
|
phys_solve_weld_joints(ctx, substep_dt);
|
|
|
|
/* Integrate */
|
|
phys_integrate_velocities(ctx, substep_dt);
|
|
|
|
/* Relaxation solve */
|
|
#if SIM_PHYSICS_ENABLE_COLLISION && SIM_PHYSICS_ENABLE_RELAXATION
|
|
phys_solve_contacts(ctx, substep_dt, 0);
|
|
#endif
|
|
}
|
|
|
|
phys_update_aabbs(ctx);
|
|
scratch_end(scratch);
|
|
}
|
|
|
|
ss->phys_iteration = phys_iteration;
|
|
}
|