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

motor joint

Browse Source
This commit is contained in:
jacob 2024-10-28 14:33:26 -05:00
parent 68d80de75a
commit 4e920e071d
7 changed files with 460 additions and 886 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

560
src/collider.c
View File

@ -7,7 +7,6 @@
u32 collider_debug_steps = U32_MAX;
//u32 collider_debug_steps = 1000000;
//u32 collider_debug_steps = 50;
#endif
INTERNAL void _dbgbreakable(void)
{
@ -21,6 +20,9 @@ INTERNAL void _dbgbreakable(void)
} else if (dbg_step >= collider_debug_steps - 1) { \
_dbgbreakable(); \
}
#else
#define DBGSTEP
#endif
struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir)
{
@ -558,15 +560,14 @@ struct collider_collision_points_result collider_collision_points(struct collide
b32 ignore_a = false;
b32 ignore_b = false;
#if 1
if (v2_len_sq(v2_sub(contact_b, contact_a)) < (0.005f * 0.005f)) {
/* Merge contacts */
if (a_sep > b_sep) {
ignore_a = true;
} else {
ignore_b = true;
}
}
#endif
if (a_sep < tolerance && !ignore_a) {
struct collider_collision_point *point = &points[num_points++];
@ -597,7 +598,9 @@ struct collider_collision_points_result collider_collision_points(struct collide
}
res.solved = true;
#if COLLIDER_DEBUG
abort:
#endif
u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points));
for (u32 i = 0; i < len; ++i) {
res.prototype.points[i] = proto[i];
@ -726,554 +729,3 @@ b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_sh
return false;
}
#endif
#if 0
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */
struct collider_collision_points_result res = ZI;
struct v2 *points0 = shape0->points;
struct v2 *points1 = shape1->points;
u32 count0 = shape0->count;
u32 count1 = shape1->count;
f32 radius0 = shape0->radius;
f32 radius1 = shape1->radius;
(UNUSED)radius0;
(UNUSED)radius1;
/* TODO: Parameterize */
const f32 tolerance = 0.005f; /* How close can shapes be before collision is considered */
const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
const u32 max_epa_iterations = 64; /* To prevent extremely large prototypes when origin is in exact center of rounded feature */
b32 colliding = false;
b32 simplex_is_closest_edge = false;
struct collider_simplex s = ZI;
struct v2 *proto = NULL;
u32 proto_count = 0;
struct v2 normal = ZI;
struct collider_collision_point points[2] = ZI;
u32 num_points = 0;
struct v2 dir = ZI;
struct v2 m = ZI;
#if COLLIDER_DEBUG
u32 dbg_step = 0;
#endif
/* ========================== *
* GJK
*
* Determine encapsulating simplex if colliding, or closest edge / point to
* origin on simplex (for check if shape distances are within tolerance)
* ========================== */
{
/* First point is support point in shape's general directions to eachother */
dir = v2_sub(xf1.og, xf0.og);
if (v2_is_zero(dir)) dir = V2(1, 0);
s.a = menkowski_point(shape0, shape1, xf0, xf1, dir);
s.len = 1;
struct v2 removed_a = ZI;
struct v2 removed_b = ZI;
u32 num_removed = 0;
while (true) {
if (s.len == 1) {
/* Second point is support point towards origin */
dir = v2_neg(s.a);
DBGSTEP;
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
/* Check that new point is far enough away from existing point */
if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) {
simplex_is_closest_edge = true;
break;
}
s.b = s.a;
s.a = m;
s.len = 2;
/* Third point is support point in direction of line normal towards origin */
dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a));
}
{
DBGSTEP;
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
/* Check that new point is far enough away from existing points */
if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq ||
v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq ||
(
(num_removed >= 1) && (
(v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) ||
(num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq))
) ||
math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) {
simplex_is_closest_edge = true;
break;
}
s.c = s.b;
s.b = s.a;
s.a = m;
s.len = 3;
if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) {
/* Simplex lies on origin */
colliding = true;
break;
}
}
/* Determine region of the simplex in which the origin lies */
DBGSTEP;
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vac = v2_sub(s.c, s.a);
struct v2 vbc = v2_sub(s.c, s.b);
struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a));
f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a));
f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b));
f32 vab_dot = v2_dot(vab, v2_neg(s.a)) / v2_len_sq(vab);
f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac);
f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc);
if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) {
/* Region ab, remove c */
num_removed = 1;
removed_a = s.c;
s.len = 2;
dir = rab_dir; /* Next third point is in direction of region ab */
} else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) {
/* Region ac, remove b */
num_removed = 1;
removed_a = s.b;
s.len = 2;
s.b = s.c;
dir = rac_dir; /* Next third point is in direction of region ac */
} else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) {
/* Region bc, remove a */
num_removed = 1;
removed_a = s.a;
s.len = 2;
s.a = s.b;
s.b = s.c;
dir = rbc_dir; /* Next third point is in direction of region bc */
} else if (vab_dot <= 0 && vac_dot <= 0) {
/* Region a, remove bc */
num_removed = 2;
removed_a = s.b;
removed_b = s.c;
s.len = 1;
} else if (vab_dot >= 1 && vbc_dot <= 0) {
/* Region b, remove ac */
num_removed = 2;
removed_a = s.a;
removed_b = s.c;
s.len = 1;
s.a = s.b;
} else if (vac_dot >= 1 && vbc_dot >= 1) {
/* Region c, remove ab */
num_removed = 2;
removed_a = s.a;
removed_b = s.b;
s.len = 1;
s.a = s.c;
} else {
/* No region, must be in simplex */
colliding = true;
break;
}
}
}
if (colliding) {
/* ========================== *
* Epa (to find collision normal from inside shape)
* ========================== */
const f32 epa_normal_epsilon_sq = 0.001f * 0.001f;
proto = arena_dry_push(scratch.arena, struct v2);
proto_count = 0;
{
ASSERT(s.len == 3);
struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3);
tmp[0] = s.a;
tmp[1] = s.b;
tmp[2] = s.c;
proto_count = 3;
}
u32 epa_iterations = 0;
while (colliding) {
++epa_iterations;
f32 pen_len_sq = F32_INFINITY;
/* Find dir from origin to closest edge */
/* FIXME: Winding order of ps & pe index */
u32 pen_ps_index = 0;
u32 pen_pe_index = 0;
struct v2 pen = ZI;
for (u32 i = 0; i < proto_count; ++i) {
u32 ps_index = i;
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
struct v2 ps = proto[ps_index];
struct v2 pe = proto[pe_index];
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse)));
struct v2 pd = v2_add(ps, vsd);
f32 pd_len_sq = v2_len_sq(pd);
if (pd_len_sq < pen_len_sq) {
pen_ps_index = ps_index;
pen_pe_index = pe_index;
pen_len_sq = pd_len_sq;
pen = pd;
}
}
/* TODO: Remove this (debugging) */
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
s.len = 2;
/* Find new point in dir */
DBGSTEP;
{
/* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */
struct v2 a = proto[pen_ps_index];
struct v2 b = proto[pen_pe_index];
struct v2 vab = v2_sub(b, a);
if (pen_len_sq < epa_normal_epsilon_sq) {
/* Next point is in direction of line normal pointing outwards from simplex */
struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */
dir = v2_perp_towards_dir(vab, v2_sub(a, n));
} else {
dir = v2_perp_towards_dir(vab, a);
}
}
m = menkowski_point(shape0, shape1, xf0, xf1, dir);
/* Check unique */
/* TODO: Better */
{
b32 unique = true;
for (u32 i = 0; i < proto_count; ++i) {
struct v2 edge_start = proto[i];
struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0];
struct v2 vsm = v2_sub(m, edge_start);
if (v2_len_sq(vsm) < min_unique_pt_dist_sq ||
math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) {
unique = false;
break;
}
}
if (!unique || epa_iterations >= max_epa_iterations) {
res.path = 1;
if (pen_len_sq < epa_normal_epsilon_sq) {
normal = v2_norm(dir);
} else {
normal = v2_norm(pen);
}
break;
}
}
/* Insert point into prototype */
/* FIXME: Preserve winding order */
arena_push(scratch.arena, struct collider_menkowski_point);
++proto_count;
for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
u32 shift_to = i;
proto[shift_to] = proto[shift_from];
}
proto[pen_pe_index] = m;
}
} else if (simplex_is_closest_edge) {
if (s.len == 1) {
struct v2 p = v2_neg(s.a);
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
} else {
/* Shapes are not overlapping (origin is outside of simplex). Project
* origin to determine if distance is within tolerance. */
ASSERT(s.len == 2);
struct v2 vab = v2_sub(s.b, s.a);
struct v2 vao = v2_neg(s.a);
f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
if (v2_len_sq(p) <= (tolerance * tolerance)) {
res.path = 2;
normal = v2_norm(dir);
colliding = true;
}
}
}
if (colliding) {
/* ========================== *
* Clip to determine final points
* ========================== */
/* Max vertices must be < 16 to fit in 4 bit ids */
CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16);
DBGSTEP;
{
//const f32 wedge_epsilon = 0.001f;
const f32 wedge_epsilon = 0.1f;
/* shape0 a -> b winding = clockwise */
u32 id_a0;
u32 id_b0;
struct v2 a0;
struct v2 b0;
/* shape1 a -> b winding = counterclockwise */
u32 id_a1;
u32 id_b1;
struct v2 a1;
struct v2 b1;
{
u32 p_i = collider_support_point_index(shape0, xf0, normal);
u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1);
u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0;
struct v2 p = xform_mul_v2(xf0, points0[p_i]);
struct v2 a = xform_mul_v2(xf0, points0[a_i]);
struct v2 b = xform_mul_v2(xf0, points0[b_i]);
struct v2 vap = v2_sub(p, a);
struct v2 vpb = v2_sub(b, p);
/* Swap a & b depending on winding order */
if (v2_wedge(vap, vpb) < 0) {
u32 tmp_u32 = a_i;
a_i = b_i;
b_i = tmp_u32;
struct v2 tmp_v2 = a;
a = b;
b = tmp_v2;
tmp_v2 = vap;
vap = v2_neg(vpb);
vpb = v2_neg(tmp_v2);
}
f32 vap_wedge = v2_wedge(vap, normal);
f32 vpb_wedge = v2_wedge(vpb, normal);
if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
id_a0 = a_i;
id_b0 = p_i;
a0 = a;
b0 = p;
} else {
id_a0 = p_i;
id_b0 = b_i;
a0 = p;
b0 = b;
}
}
{
struct v2 neg_normal = v2_neg(normal);
u32 p_i = collider_support_point_index(shape1, xf1, neg_normal);
u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0;
u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1);
struct v2 p = xform_mul_v2(xf1, points1[p_i]);
struct v2 a = xform_mul_v2(xf1, points1[a_i]);
struct v2 b = xform_mul_v2(xf1, points1[b_i]);
struct v2 vap = v2_sub(p, a);
struct v2 vpb = v2_sub(b, p);
/* Swap a & b depending on winding order */
if (v2_wedge(vap, vpb) > 0) {
u32 tmp_u32 = a_i;
a_i = b_i;
b_i = tmp_u32;
struct v2 tmp_v2 = a;
a = b;
b = tmp_v2;
tmp_v2 = vap;
vap = v2_neg(vpb);
vpb = v2_neg(tmp_v2);
}
f32 vap_wedge = v2_wedge(vap, normal);
f32 vpb_wedge = v2_wedge(vpb, normal);
if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
id_a1 = a_i;
id_b1 = p_i;
a1 = a;
b1 = p;
} else {
id_a1 = p_i;
id_b1 = b_i;
a1 = p;
b1 = b;
}
}
#if 0
#if 1
if (radius0 > 0.0) {
struct v2 scale = xform_get_scale(xf0);
struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale);
a0 = v2_add(a0, normal_radius);
b0 = v2_add(b0, normal_radius);
}
if (radius1 > 0.0) {
struct v2 scale = xform_get_scale(xf1);
struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale);
a1 = v2_sub(a1, normal_radius);
b1 = v2_sub(b1, normal_radius);
}
#else
if (radius0 > 0.0) {
struct v2 scale = xform_get_scale(xf0);
struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0), scale);
a0 = v2_add(a0, perp_radius);
b0 = v2_add(b0, perp_radius);
}
if (radius1 > 0.0) {
struct v2 scale = xform_get_scale(xf1);
struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1), scale);
a1 = v2_sub(a1, perp_radius);
b1 = v2_sub(b1, perp_radius);
}
#endif
#endif
f32 a0t = 0;
f32 a1t = 0;
f32 b0t = 0;
f32 b1t = 0;
struct v2 vab0 = v2_sub(b0, a0);
struct v2 vab1 = v2_sub(b1, a1);
{
struct v2 va0a1 = v2_sub(a1, a0);
struct v2 vb0b1 = v2_sub(b1, b0);
f32 vab0_wedge_normal = v2_wedge(vab0, normal);
f32 vab1_wedge_normal = v2_wedge(vab1, normal);
f32 va0a1_wedge_normal = v2_wedge(va0a1, normal);
f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal);
if (math_fabs(vab0_wedge_normal) > 0.01f) {
f32 w = 1 / vab0_wedge_normal;
a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1);
b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1);
}
if (math_fabs(vab1_wedge_normal) > 0.01f) {
f32 w = 1 / vab1_wedge_normal;
a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1);
b1t = clamp_f32(-vb0b1_wedge_normal * -w, 0, 1);
}
}
struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t));
struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t));
struct v2 b0_clipped = v2_add(b0, v2_mul(vab0, -b0t));
struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -b1t));
struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped);
struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped);
f32 a_sep = v2_dot(va0a1_clipped, normal);
f32 b_sep = v2_dot(vb0b1_clipped, normal);
struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
//b32 merge_contacts = v2_len_sq(v2_sub(contact_b, contact_a)) < 0.01f;
b32 merge_contacts = false;
b32 force = false;
#if 0
if (a_sep > tolerance && b_sep > tolerance) {
res.path = 999999999;
DEBUGBREAKABLE;
}
#endif
if (force || a_sep < tolerance) {
struct collider_collision_point *point = &points[num_points++];
point->id = id_a0 | (id_a1 << 4);
point->separation = a_sep;
point->point = contact_a;
}
if (force || (b_sep < tolerance && !merge_contacts)) {
struct collider_collision_point *point = &points[num_points++];
point->id = id_b0 | (id_b1 << 4);
point->separation = b_sep;
point->point = contact_b;
}
res.a0 = a0_clipped;
res.a1 = a1_clipped;
res.b0 = b0_clipped;
res.b1 = b1_clipped;
}
}
res.solved = true;
abort:
if (proto_count > 0) {
u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points));
for (u32 i = 0; i < len; ++i) {
res.prototype.points[i] = proto[i];
}
res.prototype.len = len;
} else {
if (s.len >= 1) {
res.prototype.points[0] = s.a;
if (s.len >= 2) {
res.prototype.points[1] = s.b;
if (s.len >= 3) {
res.prototype.points[2] = s.c;
}
}
}
res.prototype.len = s.len;
}
res.normal = normal;
res.points[0] = points[0];
res.points[1] = points[1];
res.num_points = num_points;
res.simplex = s;
scratch_end(scratch);
return res;
}
#endif

3
src/common.h
View File

@ -578,8 +578,11 @@ struct xform {
};
struct mat4x4 {
union {
struct { struct v4 bx, by, bz, bw; };
f32 e[4][4];
};
};
#define RECT(_x, _y, _width, _height) (struct rect) { .x = (_x), .y = (_y), .width = (_width), .height = (_height) }
#define RECT_FROM_V2(_pos, _size) (struct rect) { .pos = (_pos), .size = (_size) }

7
src/config.h
View File

@ -33,12 +33,12 @@
#define GAME_TIMESCALE 1.0
#define GAME_PHYSICS_SUBSTEPS 4
#define GAME_PHYSICS_ENABLE_WARM_STARTING 1
#define GAME_PHYSICS_ENABLE_WARM_STARTING 0
#define GAME_PHYSICS_ENABLE_RELAXATION 1
#define USER_DRAW_MENKOWSKI 0
#define GAME_PHYSICS_ENABLE_GROUND_FRICTION 1
#define GAME_PHYSICS_ENABLE_COLLISION 0
#define GAME_PHYSICS_ENABLE_COLLISION 1
#define GAME_SPAWN_LOTS 0
#define GAME_SPAWN_TESTENT 0
#define GAME_SPAWN_BOX 1
@ -54,7 +54,8 @@
#define USER_INTERP_OFFSET_TICK_RATIO 1.1
#define USER_INTERP_ENABLED 1
#define COLLIDER_DEBUG RTC
//#define COLLIDER_DEBUG RTC
#define COLLIDER_DEBUG 1
/* ========================== *
* Settings

67
src/entity.h
View File

@ -13,6 +13,7 @@ enum entity_prop {
ENTITY_PROP_PHYSICAL,
ENTITY_PROP_CONTACT_CONSTRAINT,
ENTITY_PROP_MOTOR_JOINT,
ENTITY_PROP_PLAYER_CONTROLLED,
ENTITY_PROP_CAMERA,
@ -78,7 +79,6 @@ struct contact_point {
struct contact_constraint {
struct entity_handle e0;
struct entity_handle e1;
f32 inv_m0;
f32 inv_m1;
f32 inv_i0;
@ -96,6 +96,58 @@ struct contact_constraint {
};
struct motor_joint_def {
struct entity_handle e0;
struct entity_handle e1;
f32 correction_factor;
f32 max_force;
f32 max_torque;
};
struct motor_joint {
struct entity_handle e0;
struct entity_handle e1;
f32 correction_factor;
f32 max_force;
f32 max_torque;
f32 inv_m0;
f32 inv_m1;
f32 inv_i0;
f32 inv_i1;
struct v2 linear_impulse;
f32 angular_impulse;
struct v2 point_local_e0;
struct v2 point_local_e1;
struct xform linear_mass_xf;
f32 angular_mass;
};
INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def)
{
struct motor_joint res = ZI;
res.e0 = def.e0;
res.e1 = def.e1;
res.correction_factor = clamp_f32(def.correction_factor, 0, 1);
res.max_force = def.max_force;
res.max_torque = def.max_torque;
return res;
}
struct entity {
/* ====================================================================== */
/* Metadata */
@ -135,14 +187,17 @@ struct entity {
struct collider_shape local_collider;
/* ====================================================================== */
/* Contact constraint */
/* ENTITY_PROP_CONSTRAINT_CONTACT */
struct contact_constraint contact_constraint;
struct contact_constraint contact_constraint_data;
/* ====================================================================== */
/* Motor joint */
/* ENTITY_PROP_MOTOR_JOINT */
struct motor_joint motor_joint_data;
@ -173,6 +228,8 @@ struct entity {
struct v2 focus;
} control;
struct entity_handle aim_joint;
/* ====================================================================== */
/* Physics */

652
src/game.c
View File

@ -122,6 +122,8 @@ INTERNAL void activate_now(struct entity *ent)
INTERNAL void spawn_test_entities(f32 offset)
{
struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root);
root->mass_unscaled = F32_INFINITY;
root->inertia_unscaled = F32_INFINITY;
//const f32 offset_all = -20000;
//const f32 offset_all = -5000;
@ -148,8 +150,9 @@ INTERNAL void spawn_test_entities(f32 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(0.5, 0.25);
//struct v2 size = V2(1.0, 1.0);
//struct v2 size = V2(1.5, 1.5);
//f32 r = PI;
@ -343,10 +346,10 @@ INTERNAL void spawn_test_entities(f32 offset)
/* ========================== *
* TESTING MANIFOLDS / CONTACTS
* TESTING CONTACT CONSTRAINT
* ========================== */
INTERNAL void generate_contacts(void)
INTERNAL void prepare_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. */
@ -364,7 +367,6 @@ INTERNAL void generate_contacts(void)
struct entity_store *store = G.tick.entity_store;
struct entity *root = G.root;
#if 0
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
struct entity *e0 = &store->entities[e0_index];
if (!entity_is_valid_and_active(e0)) continue;
@ -384,9 +386,11 @@ INTERNAL void generate_contacts(void)
continue;
}
/* Retrieve constraint */
u64 constraint_hash;
struct string constraint_key;
struct entity_handle *entry;
struct entity *constraint_ent = NULL;
{
{
struct entity_handle h0 = e0->handle;
struct entity_handle h1 = e1->handle;
@ -395,44 +399,43 @@ INTERNAL void generate_contacts(void)
constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash));
}
struct entity *constraint_ent = NULL;
struct entity_handle *entry = fixed_dict_get(&dict, constraint_key);
entry = fixed_dict_get(&dict, constraint_key);
if (entry) {
struct entity *t = entity_from_handle(store, *entry);
if (entity_is_valid_and_active(t)) {
if (t->contact_constraint_data.last_iteration == constraint_iteration) {
/* Constraint has already been computed this iteration */
continue;
} else {
t->contact_constraint_data.last_iteration = constraint_iteration;
constraint_ent = t;
}
}
/* Ensure constraint hasn't already been computed this iteration */
if (constraint_ent) {
if (constraint_ent->contact_constraint.last_iteration == constraint_iteration) {
/* Already iterated this constraint from The other entity's perspective, skip */
} else {
/* Constraint entity no longer valid */
fixed_dict_set(&dict_arena, &dict, constraint_key, NULL);
continue;
}
constraint_ent->contact_constraint.last_iteration = constraint_iteration;
}
}
/* Calculate entity 1 shape */
/* Calculate collision */
struct xform e1_xf = entity_get_xform(e1);
struct collider_shape e1_collider = e1->local_collider;
struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf);
struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1->local_collider, e0_xf, e1_xf);
/* Parts of algorithm are hard-coded to support 2 contact points */
CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint.points) == 2);
CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2);
CT_ASSERT(ARRAY_COUNT(res.points) == 2);
/* TODO: Move this down */
if (res.num_points > 0 || COLLIDER_DEBUG) {
if (!constraint_ent) {
constraint_ent = entity_alloc(root);
constraint_ent->contact_constraint.e1 = e1->handle;
constraint_ent->contact_constraint.e0 = e0->handle;
constraint_ent->contact_constraint_data.e1 = e1->handle;
constraint_ent->contact_constraint_data.e0 = e0->handle;
/* TODO: Should we recalculate normal as more contact points are added? */
entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT);
activate_now(constraint_ent);
if (entry) {
*entry = constraint_ent->handle;
} else {
@ -441,7 +444,8 @@ INTERNAL void generate_contacts(void)
fixed_dict_set(&dict_arena, &dict, constraint_key, entry);
}
}
data->normal = res.normal;
struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
constraint->normal = res.normal;
/* TODO: Remove this (debugging) */
#if COLLIDER_DEBUG
@ -465,11 +469,13 @@ INTERNAL void generate_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 */
/* Prepare constraint masses */
/* Calculate masses */
f32 inv_m0;
f32 inv_m1;
f32 inv_i0;
@ -481,15 +487,15 @@ INTERNAL void generate_contacts(void)
inv_m1 = 1.f / (e1->mass_unscaled * scale1);
inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
data->inv_m0 = inv_m0;
data->inv_m1 = inv_m1;
data->inv_i0 = inv_i0;
data->inv_i1 = inv_i1;
}
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 *old = &constraint->contacts[i];
struct contact_point *old = &constraint->points[i];
u32 id = old->id;
b32 found = false;
for (u32 j = 0; j < res.num_points; ++j) {
@ -500,223 +506,7 @@ INTERNAL void generate_contacts(void)
}
if (!found) {
/* Delete contact by replacing with last in array */
*old = constraint->contacts[--constraint->num_points];
--i;
}
}
/* Update / insert returned contacts */
for (u32 i = 0; i < res.num_points; ++i) {
struct collider_collision_point *res_point = &res.points[i];
struct v2 point = res_point->point;
f32 sep = res_point->separation;
u32 id = res_point->id;
struct contact *contact = NULL;
/* Match */
for (u32 j = 0; j < constraint->num_points; ++j) {
struct contact *t = &constraint->contacts[j];
if (t->id == id) {
contact = t;
break;
}
}
if (contact) {
/* Update existing */
#if !GAME_PHYSICS_ENABLE_WARM_STARTING
contact->normal_impulse = 0;
contact->tangent_impulse = 0;
#endif
} else {
/* Insert new */
contact = &constraint->contacts[constraint->num_points++];
MEMZERO_STRUCT(contact);
contact->id = id;
}
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;
}
#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) {
#if COLLIDER_DEBUG
constraint->num_points = 0;
#else
/* No longer colliding, delete constraint */
constraint->num_points = 0;
entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
#endif
}
}
}
#else
for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
struct entity *e0 = &store->entities[e0_index];
if (!entity_is_valid_and_active(e0)) continue;
if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
struct xform e0_xf = entity_get_xform(e0);
struct collider_shape e0_collider = e0->local_collider;
for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
struct entity *e1 = &store->entities[e1_index];
if (e1 == e0) continue;
if (!entity_is_valid_and_active(e1)) continue;
if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
/* TODO: Remove this (temporary stop to prevent double-constraint creation) */
if (e0_index >= e1_index) {
continue;
}
u64 constraint_hash;
struct string constraint_key;
struct entity_handle *entry;
struct entity *constraint = NULL;
{
{
struct entity_handle h0 = e0->handle;
struct entity_handle h1 = e1->handle;
constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0));
constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1));
constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash));
}
entry = fixed_dict_get(&dict, constraint_key);
if (entry) {
struct entity *t = entity_from_handle(store, *entry);
if (entity_is_valid_and_active(t)) {
if (t->contact_constraint.last_iteration == constraint_iteration) {
/* Constraint has already been computed this iteration */
continue;
} else {
t->contact_constraint.last_iteration = constraint_iteration;
constraint = t;
}
} else {
/* Constraint entity no longer valid */
continue;
}
}
}
/* Calculate collision */
struct xform e1_xf = entity_get_xform(e1);
struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1->local_collider, e0_xf, e1_xf);
/* Parts of algorithm are hard-coded to support 2 contact points */
CT_ASSERT(ARRAY_COUNT(constraint->contact_constraint.points) == 2);
CT_ASSERT(ARRAY_COUNT(res.points) == 2);
/* TODO: Move this down */
if (res.num_points > 0 || COLLIDER_DEBUG) {
if (!constraint) {
constraint = entity_alloc(root);
constraint->contact_constraint.e1 = e1->handle;
constraint->contact_constraint.e0 = e0->handle;
/* TODO: Should we recalculate normal as more contact points are added? */
entity_enable_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT);
activate_now(constraint);
if (entry) {
*entry = constraint->handle;
} else {
entry = arena_push(&dict_arena, struct entity_handle);
*entry = constraint->handle;
fixed_dict_set(&dict_arena, &dict, constraint_key, entry);
}
}
struct contact_constraint *data = &constraint->contact_constraint;
data->normal = res.normal;
/* TODO: Remove this (debugging) */
#if COLLIDER_DEBUG
{
data->res = res;
data->dbg_xf0 = e0_xf;
data->dbg_xf1 = e1_xf;
if (data->num_points == 0) {
if (res.num_points > 0) {
++e0->colliding;
++e1->colliding;
}
} else {
if (res.num_points == 0) {
--e0->colliding;
--e1->colliding;
}
}
}
#endif
}
if (res.num_points > 0) {
struct contact_constraint *data = &constraint->contact_constraint;
struct v2 normal = res.normal;
struct v2 tangent = v2_perp(normal);
/* TODO: Cache this */
/* Prepare constraint 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);
data->inv_m0 = inv_m0;
data->inv_m1 = inv_m1;
data->inv_i0 = inv_i0;
data->inv_i1 = inv_i1;
}
/* Delete old contacts that are no longer present */
for (u32 i = 0; i < data->num_points; ++i) {
struct contact_point *old = &data->points[i];
u32 id = old->id;
b32 found = false;
for (u32 j = 0; j < res.num_points; ++j) {
if (res.points[j].id == id) {
found = true;
break;
}
}
if (!found) {
/* Delete contact by replacing with last in array */
*old = data->points[--data->num_points];
*old = constraint->points[--constraint->num_points];
--i;
}
}
@ -729,8 +519,8 @@ INTERNAL void generate_contacts(void)
u32 id = res_point->id;
struct contact_point *contact = NULL;
/* Match */
for (u32 j = 0; j < data->num_points; ++j) {
struct contact_point *t = &data->points[j];
for (u32 j = 0; j < constraint->num_points; ++j) {
struct contact_point *t = &constraint->points[j];
if (t->id == id) {
contact = t;
break;
@ -744,10 +534,11 @@ INTERNAL void generate_contacts(void)
#endif
} else {
/* Insert new */
contact = &data->points[data->num_points++];
contact = &constraint->points[constraint->num_points++];
MEMZERO_STRUCT(contact);
contact->id = id;
}
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;
@ -758,7 +549,6 @@ INTERNAL void generate_contacts(void)
contact->dbg_pt = point;
}
#endif
{
struct v2 vcp0 = v2_sub(point, e0_xf.og);
struct v2 vcp1 = v2_sub(point, e1_xf.og);
@ -782,16 +572,16 @@ INTERNAL void generate_contacts(void)
}
} else if (constraint) {
constraint->contact_constraint.num_points= 0;
} else if (constraint_ent) {
constraint_ent->contact_constraint_data.num_points = 0;
#if !COLLIDER_DEBUG
/* No longer colliding, delete constraint */
entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
fixed_dict_set(&dict_arena, &dict, constraint_key, NULL);
entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
#endif
}
}
}
#endif
}
INTERNAL void warm_start_contacts(void)
@ -799,23 +589,23 @@ INTERNAL void warm_start_contacts(void)
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *constraint = &store->entities[entity_index];
if (!entity_is_valid_and_active(constraint)) continue;
if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
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 *data = &constraint->contact_constraint;
struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
u32 num_points = data->num_points;
struct entity *e0 = entity_from_handle(store, data->e0);
struct entity *e1 = entity_from_handle(store, data->e1);
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 xform e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_get_xform(e1);
f32 inv_m0 = data->inv_m0;
f32 inv_m1 = data->inv_m1;
f32 inv_i0 = data->inv_i0;
f32 inv_i1 = data->inv_i1;
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;
@ -823,11 +613,11 @@ INTERNAL void warm_start_contacts(void)
f32 w1 = e1->angular_velocity;
/* Warm start */
struct v2 normal = data->normal;
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 contact_point *point = &data->points[i];
struct contact_point *point = &constraint->points[i];
struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
@ -872,35 +662,34 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
{
struct entity_store *store = G.tick.entity_store;
for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
struct entity *constraint = &store->entities[entity_index];
if (!entity_is_valid_and_active(constraint)) continue;
if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
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 *data = &constraint->contact_constraint;
struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
struct entity *e0 = entity_from_handle(store, data->e0);
struct entity *e1 = entity_from_handle(store, data->e1);
struct entity *e0 = entity_from_handle(store, constraint->e0);
struct entity *e1 = entity_from_handle(store, 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 = data->num_points;
u32 num_points = constraint->num_points;
if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
struct xform e0_xf = entity_get_xform(e0);
struct xform e1_xf = entity_get_xform(e1);
f32 inv_m0 = data->inv_m0;
f32 inv_m1 = data->inv_m1;
f32 inv_i0 = data->inv_i0;
f32 inv_i1 = data->inv_i1;
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 = data->normal;
struct v2 normal = constraint->normal;
for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct contact_point *point = &data->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 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
@ -919,7 +708,7 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
/* Soft constraint */
f32 contact_damping_ratio = 10.0f;
f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f;
f32 contact_hertz = (1.f / dt) / 8;
struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt);
@ -955,7 +744,7 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
/* Tangent impulse */
struct v2 tangent = v2_perp(normal);
for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct contact_point *point = &data->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 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
@ -997,11 +786,184 @@ 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;
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);
if (entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
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);
}
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_invert_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 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.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 !GAME_PHYSICS_ENABLE_WARM_STARTING
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);
}
}
}
INTERNAL void warm_start_motor_joints(void)
{
}
INTERNAL void solve_motor_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_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 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_factor / dt;
/* Angular constraint */
{
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_bias = angular_separation * correction_rate;
f32 vel_diff = math_unwind_angle(w1 - w0);
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;
joint->angular_impulse = clamp_f32(joint->angular_impulse + impulse, -max_impulse, max_impulse);
impulse = joint->angular_impulse - old_impulse;
w0 -= impulse * inv_i0;
w1 += impulse * 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);
struct v2 Cdot = 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(Cdot, linear_bias)));
struct v2 old_impulse = joint->linear_impulse;
joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse);
impulse = v2_sub(joint->linear_impulse, old_impulse);
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
w0 -= v2_wedge(impulse, vcp0) * inv_i0;
w1 += v2_wedge(impulse, vcp1) * inv_i1;
}
e0->linear_velocity = v0;
e0->angular_velocity = w0;
e1->linear_velocity = v1;
e1->angular_velocity = w1;
}
}
#else
INTERNAL void prepare_motor_joints(void)
{
}
INTERNAL void warm_start_motor_joints(void)
{
}
INTERNAL void solve_motor_joints(f32 dt)
{
(UNUSED)dt;
}
#endif
@ -1030,13 +992,9 @@ INTERNAL void integrate_velocities_from_forces(f32 dt)
struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
f32 torque_accel = (ent->torque / inertia) * dt;
/* Integrate */
ent->linear_velocity = v2_add(ent->linear_velocity, force_accel);
ent->angular_velocity += torque_accel;
/* Clamp velocities */
ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY);
ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
/* Integrate & clamp */
ent->linear_velocity = v2_clamp_len(v2_add(ent->linear_velocity, force_accel), GAME_MAX_LINEAR_VELOCITY);
ent->angular_velocity = clamp_f32(ent->angular_velocity + torque_accel, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
/* Reset forces */
ent->force = V2(0, 0);
@ -1057,9 +1015,10 @@ INTERNAL void integrate_positions_from_velocities(f32 dt)
ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY);
ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY);
struct xform xf = entity_get_xform(ent);
struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt);
f32 tick_angular_velocity = ent->angular_velocity * dt;
struct xform xf = entity_get_xform(ent);
xf.og = v2_add(xf.og, tick_linear_velocity);
xf = xform_basis_rotated_world(xf, tick_angular_velocity);
entity_set_xform(ent, xf);
@ -1545,6 +1504,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* ========================== */
#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;
@ -1599,7 +1559,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
if (!F32_IS_NAN(final_xf_angle)) {
const f32 angle_error_allowed = 0.001;
f32 diff = math_diff_angle(final_xf_angle, old_angle);
f32 diff = math_unwind_angle(final_xf_angle - old_angle);
if (math_fabs(diff) > angle_error_allowed) {
xf = xform_basis_rotated_world(xf, diff);
}
@ -1608,6 +1568,96 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
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;
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);
/* Retrieve / create aim joint */
struct entity *joint_ent = entity_from_handle(store, ent->aim_joint);
if (!entity_is_valid_and_active(joint_ent)) {
joint_ent = entity_alloc(root);
entity_enable_prop(joint_ent, ENTITY_PROP_MOTOR_JOINT);
entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE);
struct motor_joint_def def = ZI;
def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */
def.e1 = ent->handle;
def.correction_factor = 0.5;
def.max_force = 0;
def.max_torque = 2500;
joint_ent->motor_joint_data = motor_joint_from_def(def);
ent->aim_joint = joint_ent->handle;
}
/* Solve for final angle using law of sines */
f32 new_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);
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
if (!F32_IS_NAN(new_angle)) {
const f32 angle_error_allowed = 0.001;
struct xform joint_xf = entity_get_xform(joint_ent);
f32 diff = math_unwind_angle(new_angle - xform_get_rotation(joint_xf));
if (math_fabs(diff) > angle_error_allowed) {
struct xform joint_ent_xf = xform_basis_with_rotation_world(joint_xf, new_angle);
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
/* ========================== *
@ -1688,25 +1738,37 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
* ========================== */
(UNUSED)generate_contacts;
(UNUSED)prepare_contacts;
(UNUSED)warm_start_contacts;
(UNUSED)solve_contacts;
(UNUSED)prepare_motor_joints;
(UNUSED)warm_start_motor_joints;
(UNUSED)solve_motor_joints;
(UNUSED)integrate_velocities_from_forces;
(UNUSED)integrate_positions_from_velocities;
(UNUSED)warm_start_contacts;
#if 1
{
integrate_velocities_from_forces(dt);
generate_contacts();
prepare_contacts();
prepare_motor_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();
#endif
#if GAME_PHYSICS_ENABLE_COLLISION
solve_contacts(substep_dt, true);
#endif
solve_motor_joints(substep_dt);
integrate_positions_from_velocities(substep_dt);
#if GAME_PHYSICS_ENABLE_COLLISION && GAME_PHYSICS_ENABLE_RELAXATION
solve_contacts(substep_dt, false); /* Relaxation */
#endif

13
src/math.h
View File

@ -579,12 +579,11 @@ INLINE f32 math_acos(f32 x)
return (PI / 2.0f) - math_atan2(x, math_sqrt(1.0f - (x * x)));
}
/* Returns wrapped difference between angles.
* E.G. diff(PI, -PI) = 0 */
INLINE f32 math_diff_angle(f32 a, f32 b)
/* Returns angle in range [-PI, PI] */
INLINE f32 math_unwind_angle(f32 a)
{
f32 diff = math_fmod(a - b, TAU);
return math_fmod(2.0f * diff, TAU) - diff;
f32 d = math_fmod(a, TAU);
return math_fmod(2.0f * d, TAU) - d;
}
/* ========================== *
@ -602,7 +601,7 @@ INLINE f64 math_lerp64(f64 val0, f64 val1, f64 t)
}
INLINE f32 math_lerp_angle(f32 a, f32 b, f32 t) {
f32 diff = math_diff_angle(b, a);
f32 diff = math_unwind_angle(b - a);
return a + diff * t;
}
@ -708,7 +707,7 @@ INLINE struct v2 v2_with_len(struct v2 a, f32 len)
INLINE struct v2 v2_clamp_len(struct v2 a, f32 max)
{
f32 l_sq = a.x * a.x + a.y * a.y;
if (l_sq > (max * max) && l_sq != 0) {
if (l_sq > max * max) {
f32 denom = max / math_sqrt(l_sq);
a.x *= denom;
a.y *= denom;

14
src/user.c
View File

@ -1035,7 +1035,7 @@ INTERNAL void user_update(void)
/* Draw collision */
#if 1
if (entity_has_prop(ent, ENTITY_PROP_CONTACT_CONSTRAINT)) {
struct contact_constraint *data = &ent->contact_constraint;
struct contact_constraint *data = &ent->contact_constraint_data;
struct entity *e0 = entity_from_handle(store, data->e0);
struct entity *e1 = entity_from_handle(store, data->e1);
struct collider_shape e0_collider = e0->local_collider;
@ -1178,8 +1178,8 @@ INTERNAL void user_update(void)
/* Draw contacts */
{
f32 radius = 5;
for (u32 i = 0; i < ent->contact_constraint.num_points; ++i) {
struct contact_point point = ent->contact_constraint.points[i];
for (u32 i = 0; i < ent->contact_constraint_data.num_points; ++i) {
struct contact_point point = ent->contact_constraint_data.points[i];
#if 0
struct v2 p0 = xform_mul_v2(e0_xf, contact.point_local_e0);
struct v2 p1 = xform_mul_v2(e1_xf, contact.point_local_e1);
@ -1202,7 +1202,7 @@ INTERNAL void user_update(void)
f32 arrow_thickness = 2;
f32 arrow_height = 5;
struct v2 start = xform_mul_v2(G.world_view, dbg_pt);
struct v2 end = xform_mul_v2(G.world_view, v2_add(dbg_pt, v2_mul(v2_norm(ent->contact_constraint.normal), len)));
struct v2 end = xform_mul_v2(G.world_view, v2_add(dbg_pt, v2_mul(v2_norm(ent->contact_constraint_data.normal), len)));
draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color);
}
#if 0
@ -1263,7 +1263,7 @@ INTERNAL void user_update(void)
u32 color_line = RGBA_32_F(1, 0, 1, 0.5);
u32 color_a = RGBA_32_F(1, 0, 0, 0.5);
u32 color_b = RGBA_32_F(0, 1, 0, 0.5);
struct collider_collision_points_result res = ent->contact_constraint.res;
struct collider_collision_points_result res = ent->contact_constraint_data.res;
{
struct v2 a = xform_mul_v2(G.world_view, res.a0);
struct v2 b = xform_mul_v2(G.world_view, res.b0);
@ -1397,7 +1397,7 @@ INTERNAL void user_update(void)
});
}
#if RTC
#if COLLIDER_DEBUG
/* Gjk steps */
{
i64 new_steps = collider_debug_steps;
@ -1478,7 +1478,7 @@ INTERNAL void user_update(void)
draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player pos: (%F, %F)"), FMT_FLOAT_P((f64)plaeyr_pos.x, 12), FMT_FLOAT_P((f64)plaeyr_pos.y, 12)));
pos.y += spacing;
#if RTC
#if COLLIDER_DEBUG
draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("collider gjk steps: %F"), FMT_UINT(collider_debug_steps)));
pos.y += spacing;
#endif