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

motor joint warm starting

Browse Source
This commit is contained in:
jacob 2024-10-28 15:12:23 -05:00
parent 4e920e071d
commit 5dde85a04a
3 changed files with 47 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/config.h
View File

@ -33,7 +33,7 @@
#define GAME_TIMESCALE 1.0 #define GAME_TIMESCALE 1.0
#define GAME_PHYSICS_SUBSTEPS 4 #define GAME_PHYSICS_SUBSTEPS 4
#define GAME_PHYSICS_ENABLE_WARM_STARTING 0 #define GAME_PHYSICS_ENABLE_WARM_STARTING 1
#define GAME_PHYSICS_ENABLE_RELAXATION 1 #define GAME_PHYSICS_ENABLE_RELAXATION 1
#define USER_DRAW_MENKOWSKI 0 #define USER_DRAW_MENKOWSKI 0

6
src/entity.h
View File

@ -103,7 +103,7 @@ struct contact_constraint {
struct motor_joint_def { struct motor_joint_def {
struct entity_handle e0; struct entity_handle e0;
struct entity_handle e1; struct entity_handle e1;
f32 correction_factor; f32 correction_rate;
f32 max_force; f32 max_force;
f32 max_torque; f32 max_torque;
}; };
@ -111,7 +111,7 @@ struct motor_joint_def {
struct motor_joint { struct motor_joint {
struct entity_handle e0; struct entity_handle e0;
struct entity_handle e1; struct entity_handle e1;
f32 correction_factor; f32 correction_rate;
f32 max_force; f32 max_force;
f32 max_torque; f32 max_torque;
@ -135,7 +135,7 @@ INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def)
struct motor_joint res = ZI; struct motor_joint res = ZI;
res.e0 = def.e0; res.e0 = def.e0;
res.e1 = def.e1; res.e1 = def.e1;
res.correction_factor = clamp_f32(def.correction_factor, 0, 1); res.correction_rate = clamp_f32(def.correction_rate, 0, 1);
res.max_force = def.max_force; res.max_force = def.max_force;
res.max_torque = def.max_torque; res.max_torque = def.max_torque;
return res; return res;

185
src/game.c
View File

@ -184,7 +184,7 @@ INTERNAL void spawn_test_entities(f32 offset)
//e->control_force = 5000; //e->control_force = 5000;
e->control_force = 250; e->control_force = 250;
#endif #endif
e->control_torque = 10; e->control_torque = 500;
e->control.focus = V2(0, -1); e->control.focus = V2(0, -1);
entity_enable_prop(e, ENTITY_PROP_PHYSICAL); entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
@ -371,6 +371,7 @@ INTERNAL void prepare_contacts(void)
struct entity *e0 = &store->entities[e0_index]; struct entity *e0 = &store->entities[e0_index];
if (!entity_is_valid_and_active(e0)) continue; if (!entity_is_valid_and_active(e0)) continue;
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
if (e0->local_collider.count <= 0) continue;
struct xform e0_xf = entity_get_xform(e0); struct xform e0_xf = entity_get_xform(e0);
struct collider_shape e0_collider = e0->local_collider; struct collider_shape e0_collider = e0->local_collider;
@ -380,6 +381,7 @@ INTERNAL void prepare_contacts(void)
if (e1 == e0) continue; if (e1 == e0) continue;
if (!entity_is_valid_and_active(e1)) continue; if (!entity_is_valid_and_active(e1)) continue;
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
if (e1->local_collider.count <= 0) continue;
/* TODO: Remove this (temporary stop to prevent double-constraint creation) */ /* TODO: Remove this (temporary stop to prevent double-constraint creation) */
if (e0_index >= e1_index) { if (e0_index >= e1_index) {
@ -426,7 +428,6 @@ INTERNAL void prepare_contacts(void)
CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2); CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2);
CT_ASSERT(ARRAY_COUNT(res.points) == 2); CT_ASSERT(ARRAY_COUNT(res.points) == 2);
/* TODO: Move this down */ /* TODO: Move this down */
if (res.num_points > 0 || COLLIDER_DEBUG) { if (res.num_points > 0 || COLLIDER_DEBUG) {
if (!constraint_ent) { if (!constraint_ent) {
@ -618,10 +619,8 @@ INTERNAL void warm_start_contacts(void)
f32 inv_num_points = 1.f / num_points; f32 inv_num_points = 1.f / num_points;
for (u32 i = 0; i < num_points; ++i) { for (u32 i = 0; i < num_points; ++i) {
struct contact_point *point = &constraint->points[i]; struct contact_point *point = &constraint->points[i];
struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og);
struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og);
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse));
impulse = v2_mul(impulse, inv_num_points); impulse = v2_mul(impulse, inv_num_points);
@ -745,10 +744,8 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
struct v2 tangent = v2_perp(normal); struct v2 tangent = v2_perp(normal);
for (u32 point_index = 0; point_index < num_points; ++point_index) { for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct contact_point *point = &constraint->points[point_index]; struct contact_point *point = &constraint->points[point_index];
struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og);
struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og);
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
@ -835,8 +832,8 @@ INTERNAL void prepare_motor_joints(void)
joint->point_local_e0 = V2(0, 0); joint->point_local_e0 = V2(0, 0);
joint->point_local_e1 = V2(0, 0); joint->point_local_e1 = V2(0, 0);
struct v2 vcp0 = v2_sub(xform_invert_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
struct v2 vcp1 = v2_sub(xform_invert_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);
struct xform linear_mass_xf; struct xform linear_mass_xf;
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.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1;
@ -860,6 +857,33 @@ INTERNAL void prepare_motor_joints(void)
INTERNAL void warm_start_motor_joints(void) INTERNAL void warm_start_motor_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_MOTOR_JOINT)) continue;
struct motor_joint *joint = &joint_ent->motor_joint_data;
struct entity *e0 = entity_from_handle(store, joint->e0);
struct entity *e1 = entity_from_handle(store, joint->e1);
struct xform e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_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);
e0->linear_velocity = v2_sub(e0->linear_velocity, v2_mul(joint->linear_impulse, inv_m0));
e1->linear_velocity = v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse, inv_m1));
e0->angular_velocity -= (v2_wedge(joint->linear_impulse, vcp0) + joint->angular_impulse) * inv_i0;
e1->angular_velocity += (v2_wedge(joint->linear_impulse, vcp1) + joint->angular_impulse) * inv_i1;
}
} }
INTERNAL void solve_motor_joints(f32 dt) INTERNAL void solve_motor_joints(f32 dt)
@ -888,26 +912,16 @@ INTERNAL void solve_motor_joints(f32 dt)
f32 w0 = e0->angular_velocity; f32 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity; f32 w1 = e1->angular_velocity;
f32 correction_rate = joint->correction_factor / dt; f32 correction_rate = joint->correction_rate / dt;
/* Angular constraint */ /* Angular constraint */
{ {
f32 max_impulse = joint->max_torque * dt; f32 max_impulse = joint->max_torque * dt;
#if 0
f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf));
f32 angular_bias = angular_separation * correction_rate; f32 angular_bias = angular_separation * correction_rate;
f32 vel_diff = math_unwind_angle(w1 - w0); f32 impulse = -joint->angular_mass * (w1 - w0 + angular_bias);
f32 impulse = -joint->angular_mass * (vel_diff + angular_bias);
#else
f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf));
f32 angular_bias = angular_separation * correction_rate;
//f32 vel_diff = w1 - w0;
f32 vel_diff = 0;
f32 impulse = -joint->angular_mass * (vel_diff + angular_bias);
#endif
f32 old_impulse = joint->angular_impulse; f32 old_impulse = joint->angular_impulse;
joint->angular_impulse = clamp_f32(joint->angular_impulse + impulse, -max_impulse, max_impulse); joint->angular_impulse = clamp_f32(joint->angular_impulse + impulse, -max_impulse, max_impulse);
@ -1504,71 +1518,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* ========================== */ * ========================== */
#if GAME_PLAYER_AIM #if GAME_PLAYER_AIM
#if 0
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(ent)) continue;
if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
struct xform xf = entity_get_xform(ent);
struct xform sprite_xf = xform_mul(xf, ent->sprite_local_xform);
f32 old_angle = xform_get_rotation(xf);
/* Solve for final angle using law of sines */
f32 final_xf_angle;
{
struct v2 ent_pos = xf.og;
struct v2 focus_pos = v2_add(ent_pos, ent->control.focus);
struct v2 sprite_hold_pos;
struct v2 sprite_hold_dir;
{
struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("attach.wep"), ent->animation_frame);
sprite_hold_pos = slice.center;
sprite_hold_dir = slice.dir;
}
struct v2 hold_dir = xform_basis_mul_v2(sprite_xf, sprite_hold_dir);
struct v2 hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos);
if (v2_eq(hold_pos, ent_pos)) {
/* If hold pos is same as origin (E.G if pivot is being used as hold pos), then move hold pos forward a tad to avoid issue */
sprite_hold_pos = v2_add(sprite_hold_pos, V2(0, -1));
hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos);
}
f32 forward_hold_angle_offset;
{
struct xform xf_unrotated = xform_basis_with_rotation_world(xf, 0);
struct v2 hold_pos_unrotated = xform_mul_v2(xf_unrotated, xform_mul_v2(ent->sprite_local_xform, sprite_hold_pos));
forward_hold_angle_offset = v2_angle_from_dirs(V2(0, -1), v2_sub(hold_pos_unrotated, xf_unrotated.og));
}
struct v2 hold_ent_dir = v2_sub(ent_pos, hold_pos);
struct v2 focus_ent_dir = v2_sub(ent_pos, focus_pos);
f32 hold_ent_len = v2_len(hold_ent_dir);
f32 focus_ent_len = v2_len(focus_ent_dir);
f32 final_hold_angle_btw_ent_and_focus = v2_angle_from_dirs(hold_ent_dir, hold_dir);
f32 final_focus_angle_btw_ent_and_hold = math_asin((math_sin(final_hold_angle_btw_ent_and_focus) * hold_ent_len) / focus_ent_len);
f32 final_ent_angle_btw_focus_and_hold = PI - (final_focus_angle_btw_ent_and_hold + final_hold_angle_btw_ent_and_focus);
final_xf_angle = v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset;
}
if (!F32_IS_NAN(final_xf_angle)) {
const f32 angle_error_allowed = 0.001;
f32 diff = math_unwind_angle(final_xf_angle - old_angle);
if (math_fabs(diff) > angle_error_allowed) {
xf = xform_basis_rotated_world(xf, diff);
}
}
entity_set_xform(ent, xf);
}
}
#else
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index]; struct entity *ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(ent)) continue; if (!entity_is_valid_and_active(ent)) continue;
@ -1581,17 +1530,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
struct entity *joint_ent = entity_from_handle(store, ent->aim_joint); struct entity *joint_ent = entity_from_handle(store, ent->aim_joint);
if (!entity_is_valid_and_active(joint_ent)) { if (!entity_is_valid_and_active(joint_ent)) {
joint_ent = entity_alloc(root); joint_ent = entity_alloc(root);
entity_enable_prop(joint_ent, ENTITY_PROP_PHYSICAL);
entity_enable_prop(joint_ent, ENTITY_PROP_MOTOR_JOINT); entity_enable_prop(joint_ent, ENTITY_PROP_MOTOR_JOINT);
entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE);
struct motor_joint_def def = ZI; struct motor_joint_def def = ZI;
def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */ def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */
def.e1 = ent->handle; def.e1 = ent->handle;
def.correction_factor = 0.5; def.correction_rate = 1.0;
def.max_force = 0; def.max_force = 0;
def.max_torque = 2500; def.max_torque = ent->control_torque;
joint_ent->motor_joint_data = motor_joint_from_def(def); joint_ent->motor_joint_data = motor_joint_from_def(def);
ent->aim_joint = joint_ent->handle; ent->aim_joint = joint_ent->handle;
} }
@ -1638,26 +1587,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
new_angle = math_unwind_angle(v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset); new_angle = math_unwind_angle(v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset);
} }
#if 1 joint_ent->angular_velocity = 0;
if (!F32_IS_NAN(new_angle)) { if (!F32_IS_NAN(new_angle)) {
const f32 angle_error_allowed = 0.001; const f32 angle_error_allowed = 0.001;
struct xform joint_xf = entity_get_xform(joint_ent); struct xform joint_xf = entity_get_xform(joint_ent);
f32 diff = math_unwind_angle(new_angle - xform_get_rotation(joint_xf)); f32 diff = math_unwind_angle(new_angle - xform_get_rotation(joint_xf));
if (math_fabs(diff) > angle_error_allowed) { if (math_fabs(diff) > angle_error_allowed) {
struct xform joint_ent_xf = xform_basis_with_rotation_world(joint_xf, new_angle); joint_ent->angular_velocity = diff / dt;
entity_set_xform(joint_ent, joint_ent_xf);
} }
} }
#endif
#if 0
struct xform joint_ent_xf = xform_basis_with_rotation_world(entity_get_xform(joint_ent), new_angle);
entity_set_xform(joint_ent, joint_ent_xf);
#endif
} }
} }
#endif
#endif #endif
/* ========================== * /* ========================== *
@ -1806,7 +1746,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* Update camera position * Update camera position
* ========================== */ * ========================== */
#if 0
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index]; struct entity *ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(ent)) continue; if (!entity_is_valid_and_active(ent)) continue;
@ -1843,44 +1782,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1; ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1;
entity_set_xform(ent, xf); entity_set_xform(ent, xf);
} }
#else
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *ent = &store->entities[entity_index];
if (!entity_is_valid_and_active(ent)) continue;
/* Camera follow */
if (entity_has_prop(ent, ENTITY_PROP_CAMERA)) {
struct entity *follow = entity_from_handle(store, ent->camera_follow);
struct xform xf = entity_get_xform(ent);
f32 aspect_ratio = 1.0;
{
struct xform quad_xf = xform_mul(entity_get_xform(ent), ent->camera_quad_xform);
struct v2 camera_size = xform_get_scale(quad_xf);
if (!v2_is_zero(camera_size)) {
aspect_ratio = camera_size.x / camera_size.y;
}
}
f32 ratio_y = 0.33f;
f32 ratio_x = ratio_y / aspect_ratio;
struct v2 camera_focus_dir = v2_mul_v2(follow->control.focus, V2(ratio_x, ratio_y));
struct v2 camera_focus_pos = v2_add(entity_get_xform(follow).og, camera_focus_dir);
ent->camera_xform_target = xf;
ent->camera_xform_target.og = camera_focus_pos;
/* Lerp camera */
if (ent->camera_applied_lerp_continuity_gen_plus_one == ent->camera_lerp_continuity_gen + 1) {
f32 t = 1 - math_pow(2.f, -20.f * (f32)G.tick.dt);
xf = xform_lerp(xf, ent->camera_xform_target, t);
} else {
/* Skip lerp */
xf = ent->camera_xform_target;
}
ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1;
entity_set_xform(ent, xf);
}
#endif
} }
/* ========================== * /* ========================== *