jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

start mouse joints. separate contact creation & preparation

Browse Source
This commit is contained in:
jacob 2024-10-29 13:26:32 -05:00
parent 483a95dfca
commit d0089329e2
3 changed files with 317 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/entity.h
View File

@ -14,6 +14,7 @@ enum entity_prop {
ENTITY_PROP_PHYSICAL,
ENTITY_PROP_CONTACT_CONSTRAINT,
ENTITY_PROP_MOTOR_JOINT,
ENTITY_PROP_MOUSE_JOINT,
ENTITY_PROP_PLAYER_CONTROLLED,
ENTITY_PROP_CAMERA,
@ -58,6 +59,7 @@ struct entity_store {
/* TODO: Remove this */
#include "collider.h"
#include "math.h"
struct contact_point {
/* Contact point in local space of each entity */
@ -89,6 +91,9 @@ struct contact_constraint {
struct contact_point points[2];
u32 num_points;
struct math_spring_result softness;
f32 pushout_velocity;
/* TODO: Remove this (debugging) */
struct collider_collision_points_result res;
struct xform dbg_xf0;
@ -146,6 +151,31 @@ INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def)
struct mouse_joint {
struct entity_handle target;
struct v2 point_local_target;
struct v2 point_local_mouse;
f32 max_force;
f32 inv_m;
f32 inv_i;
struct v2 linear_impulse;
f32 angular_impulse;
struct xform linear_mass_xf;
struct math_spring_result linear_softness;
struct math_spring_result angular_softness;
};
struct entity {
@ -199,6 +229,12 @@ struct entity {
/* ENTITY_PROP_MOTOR_JOINT */
struct motor_joint motor_joint_data;
/* ====================================================================== */
/* Mouse joint */
/* ENTITY_PROP_MOUSE_JOINT */
struct mouse_joint mouse_joint_data;

381
src/game.c
View File

@ -138,44 +138,39 @@ INTERNAL void spawn_test_entities(f32 offset)
struct entity *e = entity_alloc(root);
//struct v2 pos = V2(0.25, -10);
//struct v2 pos = V2(0.25, -7);
//struct v2 pos = V2(0.25, -5.27);
//struct v2 pos = V2(0.5, -1.5);
struct v2 pos = V2(1, -1);
//struct v2 pos = V2(0.300121694803, -1.322724342346);
//struct v2 pos = V2(1.0295, -1);
pos = v2_add(pos, V2(0, offset));
pos = v2_add(pos, V2(0, offset_all));
//struct v2 size = V2(1, 1);
//struct v2 size = V2(0.25, 0.25);
//struct v2 size = V2(0.5, 0.5);
//struct v2 size = V2(0.5, 0.25);
struct v2 size = V2(1.0, 1.0);
//struct v2 size = V2(1.5, 1.5);
//f32 r = PI;
f32 r = PI / 4;
//f32 r = PI / 3;
//f32 r = 0.05;
//f32 r = PI / 2;
//f32 r = PI;
//f32 r = 0;
//f32 r = PI / 4;
f32 r = 0;
if (!G.extra_spawn) {
e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
} else {
e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
size = V2(0.5, 0.5);
}
//e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase"));
//e->sprite_span_name = STR("idle.unarmed");
//e->sprite_span_name = STR("idle.one_handed");
e->sprite_span_name = STR("idle.two_handed");
struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size);
//xf.bx.y = -1.f;
entity_set_xform(e, xf);
e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
//e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
//e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase"));
//e->sprite_span_name = STR("idle.unarmed");
//e->sprite_span_name = STR("idle.one_handed");
e->sprite_span_name = STR("idle.two_handed");
entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
if (!G.extra_spawn) {
entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
}
e->linear_ground_friction = 150;
e->angular_ground_friction = 100;
@ -309,8 +304,8 @@ INTERNAL void spawn_test_entities(f32 offset)
struct v2 size = V2(1, 1);
#else
//struct v2 size = V2(5000, 1);
//struct v2 size = V2(50, 1);
struct v2 size = V2(1, 1);
struct v2 size = V2(50, 1);
//struct v2 size = V2(1, 1);
#endif
//f32 rot = PI / 4;
f32 rot = 0;
@ -369,7 +364,7 @@ INTERNAL void spawn_test_entities(f32 offset)
* TESTING CONTACT CONSTRAINT
* ========================== */
INTERNAL void prepare_contacts(void)
INTERNAL void create_contacts(void)
{
/* TODO: Remove this */
/* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */
@ -492,28 +487,6 @@ INTERNAL void prepare_contacts(void)
if (res.num_points > 0) {
struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
struct v2 normal = res.normal;
struct v2 tangent = v2_perp(normal);
/* 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);
}
constraint->inv_m0 = inv_m0;
constraint->inv_m1 = inv_m1;
constraint->inv_i0 = inv_i0;
constraint->inv_i1 = inv_i1;
/* Delete old contacts that are no longer present */
for (u32 i = 0; i < constraint->num_points; ++i) {
struct contact_point *old = &constraint->points[i];
@ -547,58 +520,101 @@ INTERNAL void prepare_contacts(void)
break;
}
}
if (contact) {
/* Update existing */
#if !GAME_PHYSICS_ENABLE_WARM_STARTING
contact->normal_impulse = 0;
contact->tangent_impulse = 0;
#endif
} else {
/* Insert new */
if (!contact) {
/* Insert */
contact = &constraint->points[constraint->num_points++];
MEMZERO_STRUCT(contact);
contact->id = id;
f32 substep_dt = (1.f / ((f32)GAME_FPS * (f32)GAME_PHYSICS_SUBSTEPS));
f32 damping_ratio = 10.0f;
f32 frequency = (1.f / substep_dt) / 8.f;
constraint->softness = math_spring(frequency, damping_ratio, substep_dt);
constraint->pushout_velocity = 3.0f;
}
/* Update points & separation */
contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point);
contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point);
contact->starting_separation = sep;
/* TODO: Remove this (debugging) */
#if COLLIDER_DEBUG
{
contact->dbg_pt = point;
}
contact->dbg_pt = point;
#endif
{
struct v2 vcp0 = v2_sub(point, e0_xf.og);
struct v2 vcp1 = v2_sub(point, e1_xf.og);
/* 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;
}
}
}
} else if (constraint_ent) {
constraint_ent->contact_constraint_data.num_points = 0;
#if !COLLIDER_DEBUG
/* No longer colliding, delete constraint */
fixed_dict_set(&dict_arena, &dict, constraint_key, NULL);
entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
}
}
}
}
INTERNAL void prepare_contacts(void)
{
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *constraint_ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(constraint_ent)) continue;
if (!entity_has_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
u32 num_points = constraint->num_points;
struct entity *e0 = entity_from_handle(store, constraint->e0);
struct entity *e1 = entity_from_handle(store, constraint->e1);
if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
struct v2 normal = constraint->normal;
struct v2 tangent = v2_perp(normal);
struct xform e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_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);
}
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 contact_point *contact = &constraint->points[i];
struct v2 vcp0 = v2_sub(xform_basis_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og);
struct v2 vcp1 = v2_sub(xform_basis_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og);
/* 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 !GAME_PHYSICS_ENABLE_WARM_STARTING
contact->normal_impulse = 0;
contact->tangent_impulse = 0;
#endif
}
}
@ -659,24 +675,6 @@ INTERNAL void warm_start_contacts(void)
}
}
struct soft_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h)
{
if (hertz == 0.0f) {
return (struct soft_result) { .mass_scale = 1.0f };
} else {
f32 omega = 2.0f * PI * hertz;
f32 a1 = 2.0f * zeta + h * omega;
f32 a2 = h * omega * a1;
f32 a3 = 1.0f / (1.0f + a2);
return (struct soft_result) {
.bias_rate = omega / a1,
.mass_scale = a2 * a3,
.impulse_scale = 1.0f / (1.0f + a2)
};
}
}
INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
{
struct entity_store *store = G.tick.entity_store;
@ -725,17 +723,11 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
velocity_bias = separation / dt;
} else if (apply_bias) {
/* Soft constraint */
f32 contact_damping_ratio = 10.0f;
f32 contact_hertz = (1.f / dt) / 8;
struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt);
f32 contact_pushout_velocity = 3.0f;
velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity);
struct math_spring_result softness = constraint->softness;
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));
@ -811,8 +803,6 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
* TESTING MOTOR JOINT
* ========================== */
#if 1
INTERNAL void prepare_motor_joints(void)
{
struct entity_store *store = G.tick.entity_store;
@ -868,9 +858,6 @@ INTERNAL void prepare_motor_joints(void)
joint->linear_impulse = V2(0, 0);
joint->angular_impulse = 0;
#endif
} else {
entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE);
entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
}
}
}
@ -979,20 +966,156 @@ INTERNAL void solve_motor_joints(f32 dt)
e1->linear_velocity = v1;
e1->angular_velocity = w1;
}
}
#if 1
/* ========================== *
* TESTING MOUSE JOINT
* ========================== */
INTERNAL void create_mouse_joints(void)
{
}
INTERNAL void prepare_mouse_joints(void)
{
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *joint_ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(joint_ent)) continue;
if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
struct mouse_joint *joint = &joint_ent->mouse_joint_data;
struct entity *ent = entity_from_handle(store, joint->target);
if (entity_is_valid_and_active(ent)) {
struct xform xf = entity_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_target), xf.og);
struct xform linear_mass_xf;
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 !GAME_PHYSICS_ENABLE_WARM_STARTING
joint->linear_impulse = V2(0, 0);
joint->angular_impulse = 0;
#endif
}
}
}
INTERNAL void warm_start_mouse_joints(void)
{
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *joint_ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(joint_ent)) continue;
if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
struct mouse_joint *joint = &joint_ent->mouse_joint_data;
struct entity *ent = entity_from_handle(store, joint->target);
if (entity_is_valid_and_active(ent)) {
f32 inv_m = joint->inv_m;
f32 inv_i = joint->inv_i;
struct xform xf = entity_get_xform(ent);
struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_target), xf.og);
ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m));
ent->angular_velocity += (v2_wedge(joint->linear_impulse, vcp) + joint->angular_impulse) * inv_i;
}
}
}
INTERNAL void solve_mouse_joints(f32 dt)
{
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *joint_ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(joint_ent)) continue;
if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
struct mouse_joint *joint = &joint_ent->mouse_joint_data;
struct entity *ent = entity_from_handle(store, joint->target);
if (entity_is_valid_and_active(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_result softness = joint->angular_softness;
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 = entity_get_xform(ent);
struct v2 point_target = xform_mul_v2(xf, joint->point_local_target);
struct v2 point_mouse = xform_mul_v2(xf, joint->point_local_mouse);
struct v2 vcp = v2_sub(point_target, xf.og);
struct v2 separation = v2_sub(point_mouse, point_target);
struct math_spring_result softness = joint->linear_softness;
struct v2 bias = v2_mul(separation, softness.bias_rate);
f32 mass_scale = softness.mass_scale;
f32 impulse_scale = softness.impulse_scale;
struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
struct v2 b = v2_mul(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)), mass_scale);
struct v2 old_impulse = joint->linear_impulse;
struct v2 impulse = v2_add(v2_mul(joint->linear_impulse, impulse_scale), b);
joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse);
impulse = v2_sub(joint->linear_impulse, old_impulse);
v = v2_add(v, v2_mul(impulse, inv_m));
w += v2_wedge(impulse, vcp) * inv_i;
}
}
}
}
#else
INTERNAL void prepare_motor_joints(void)
INTERNAL void prepare_mouse_joints(void)
{
}
INTERNAL void warm_start_motor_joints(void)
INTERNAL void warm_start_mouse_joints(void)
{
}
INTERNAL void solve_motor_joints(f32 dt)
INTERNAL void solve_mouse_joints(f32 dt)
{
(UNUSED)dt;
}
@ -1003,8 +1126,6 @@ INTERNAL void solve_motor_joints(f32 dt)
/* ========================== *
* TESTING PHYSICS INTEGRATION
* ========================== */
@ -1544,7 +1665,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
}
entity_set_xform(joint_ent, XFORM_IDENT); /* Reset joint ent position */
joint_ent->linear_velocity = v2_with_len(ent->control.move, ent->control_force_max_speed);
joint_ent->linear_velocity = v2_mul(v2_clamp_len(ent->control.move, 1), ent->control_force_max_speed);
}
}
#endif
@ -1718,7 +1839,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* Physics
* ========================== */
(UNUSED)create_contacts;
(UNUSED)prepare_contacts;
(UNUSED)warm_start_contacts;
(UNUSED)solve_contacts;
@ -1727,26 +1848,36 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
(UNUSED)warm_start_motor_joints;
(UNUSED)solve_motor_joints;
(UNUSED)create_mouse_joints;
(UNUSED)prepare_mouse_joints;
(UNUSED)warm_start_mouse_joints;
(UNUSED)solve_mouse_joints;
(UNUSED)integrate_velocities_from_forces;
(UNUSED)integrate_positions_from_velocities;
#if 1
{
integrate_velocities_from_forces(dt);
create_contacts();
create_mouse_joints();
prepare_contacts();
prepare_motor_joints();
prepare_mouse_joints();
f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS;
for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) {
#if GAME_PHYSICS_ENABLE_WARM_STARTING
warm_start_contacts();
warm_start_motor_joints();
warm_start_mouse_joints();
#endif
#if GAME_PHYSICS_ENABLE_COLLISION
solve_contacts(substep_dt, true);
#endif
solve_motor_joints(substep_dt);
solve_mouse_joints(substep_dt);
integrate_positions_from_velocities(substep_dt);

25
src/math.h
View File

@ -1193,4 +1193,29 @@ INLINE struct v2 math_poly_center(struct v2_array a)
return v2_div(sum, a.count);
}
/* ========================== *
* Other
* ========================== */
/* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */
struct math_spring_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
INLINE struct math_spring_result math_spring(f32 hertz, f32 damping_ratio, f32 dt)
{
struct math_spring_result res;
if (hertz == 0.0f) {
res.bias_rate = 0;
res.mass_scale = 1;
res.impulse_scale = 0;
} else {
f32 angular_frequency = TAU * hertz;
f32 a = 2 * damping_ratio + angular_frequency * dt;
f32 b = angular_frequency * a * dt;
f32 c = 1 / (b + 1);
res.bias_rate = angular_frequency / a;
res.mass_scale = b * c;
res.impulse_scale = c;
}
return res;
}
#endif