minor cleanup

This commit is contained in:
jacob 2024-09-09 12:08:28 -05:00
parent 5c03288c53
commit 8050c39496
4 changed files with 52 additions and 211 deletions

View File

@ -734,6 +734,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
velocity = v2_sub(xf1.og, xf0.og);
//velocity = V2(0.014992147684, -0.000010356307);
//velocity = V2(-0.0001, -0.01);
//velocity = V2(0.005, 0);
//velocity = V2(-500, -500);
@ -746,7 +747,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
xf1.og = v2_add(xf0.og, velocity);
}
struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, xf0, xf1);
struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly, velocity);
colliding = res.colliding;
point0 = res.p0;
@ -758,16 +759,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
solved = res.solved;
if (colliding) {
#if 1
any_collision = true;
pen = v2_sub(point1, point0);
/* Pen movement test */ #if 0
f32 epsilon = 0.000100; pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon));
#endif
//xf0.og = v2_add(xf0.og, pen);
//e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
//e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen);
#endif
}
}

238
src/gjk.c
View File

@ -269,13 +269,8 @@ struct poly_support_test_result {
struct v2 original;
};
#if 1
INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1)
INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct v2 linear_velocity)
{
(UNUSED)xf0;
(UNUSED)xf1;
/* TODO: Could probably binary search for largest dot since shape is convex */
struct v2 furthest = V2(0, 0);
struct v2 furthest_original = V2(0, 0);
@ -291,19 +286,17 @@ INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, st
}
}
struct v2 velocity = v2_sub(xf1.og, xf0.og);
//struct v2 velocity = V2(1, 1);
for (u32 i = 0; i < a.count; ++i) {
struct v2 p = a.points[i];
struct v2 modified;
#if 0
if (v2_dot(velocity, dir) > 0) {
modified = v2_add(p, velocity);
if (v2_dot(linear_velocity, dir) > 0) {
modified = v2_add(p, linear_velocity);
} else {
modified = p;
}
#else
modified = v2_add(p, velocity);
modified = v2_add(p, linear_velocity);
#endif
f32 dot = v2_dot(dir, modified);
if (dot > furthest_dot) {
@ -318,86 +311,27 @@ INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, st
res.original = furthest_original;
return res;
}
#else
INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1)
{
f32 epsilon = 0.001;
(UNUSED)xf0;
(UNUSED)xf1;
/* TODO: Could probably binary search for largest dot since shape is convex */
struct v2 furthest = V2(0, 0);
struct v2 furthest_original = V2(0, 0);
f32 furthest_wedge = -F32_INFINITY;
f32 furthest_dot = -F32_INFINITY;
for (u32 i = 0; i < a.count; ++i) {
struct v2 p = a.points[i];
f32 dot = v2_dot(dir, p);
f32 wedge = v2_wedge(dir, p);
if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) {
furthest = p;
furthest_original = p;
furthest_dot = dot;
furthest_wedge = wedge;
}
}
struct v2 velocity = v2_sub(xf1.og, xf0.og);
for (u32 i = 0; i < a.count; ++i) {
struct v2 p = a.points[i];
struct v2 modified;
#if 0
if (v2_dot(velocity, dir) > 0) {
modified = v2_add(p, velocity);
} else {
modified = p;
}
#else
modified = v2_add(p, velocity);
#endif
f32 dot = v2_dot(dir, modified);
f32 wedge = v2_wedge(dir, modified);
if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) {
furthest = modified;
furthest_original = p;
furthest_dot = dot;
furthest_wedge = wedge;
}
}
struct poly_support_test_result res = { 0 };
res.p = furthest;
res.original = furthest_original;
return res;
}
#endif
INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct xform xf0, struct xform xf1)
INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity)
{
struct gjk_menkowski_point res;
struct poly_support_test_result res0 = poly_support_test(poly0, dir, xf0, xf1);
struct poly_support_test_result res1 = poly_support_test(poly1, v2_neg(dir), XFORM_IDENT, XFORM_IDENT);
struct poly_support_test_result res0 = poly_support_test(poly0, dir, linear_velocity);
struct poly_support_test_result res1 = poly_support_test(poly1, v2_neg(dir), V2(0, 0));
res.p0 = res0.original;
res.p1 = res1.original;
res.p = v2_sub(res0.p, res1.p);
return res;
}
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1)
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity)
{
(UNUSED)xf0;
(UNUSED)xf1;
struct v2 velocity = v2_sub(xf1.og, xf0.og);
struct temp_arena scratch = scratch_begin_no_conflict();
struct gjk_extended_result res = { 0 };
/* FIXME: Divs by 0 */
/* TODO: Verify epsilon */
f32 epsilon = 0.00000001;
f32 epsilon = 0.00001;
struct gjk_simplex s = {
.len = 3,
.a = V2(F32_NAN, F32_NAN),
@ -408,8 +342,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
struct v2 shape0_p = { 0 };
struct v2 shape1_p = { 0 };
b32 velocity_intersects = false;
b32 use_penetration_dir = false;
(UNUSED)use_penetration_dir;
f32 velocity_intersection = 0;
/* TODO: Move this back down */
struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
@ -429,7 +362,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
/* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
{
/* First point is support point in shape's general directions to eachother */
s.a = menkowski_point_extended_test(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), xf0, xf1);
s.a = menkowski_point_extended_test(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity);
s.len = 1;
while (!colliding) {
@ -438,7 +371,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
dir = v2_neg(s.a.p);
DBGSTEP;
m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1);
m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity);
if (v2_eq(m.p, s.a.p)) {
/* Point is the same */
break;
@ -446,20 +379,17 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
s.b = s.a;
s.a = m;
s.len = 2;
#if 1
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
/* New ab lies on origin */
break;
}
#endif
/* Third point is support point in direction of line normal towards origin */
dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
}
{
DBGSTEP;
m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1);
m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity);
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) {
/* New point is on existing line ab */
break;
@ -475,7 +405,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
s.b = s.a;
s.a = m;
s.len = 3;
#if 1
if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
/* New ab lies on origin */
s.len = 2;
@ -487,7 +416,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
s.len = 2;
break;
}
#endif
}
DBGSTEP;
@ -541,7 +469,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
/* ========================== *
* Epa
* ========================== */
use_penetration_dir = !v2_eq(velocity, V2(0, 0));
proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
proto_count = 0;
{
@ -553,7 +480,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
proto_count = 3;
}
if (!use_penetration_dir) {
if (v2_eq(linear_velocity, V2(0, 0))) {
/* ========================== *
* Expand simplex towards closest edge to origin inside menkowski
* ========================== */
@ -595,7 +522,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
s.len = 2;
/* Find new point in dir */
m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1);
m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity);
/* Check unique */
/* TODO: Better */
@ -634,42 +561,11 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
DBGSTEP;
/* FIXME: Winding order of ps & pe index */
#if 0
f32 furthest = -F32_INFINITY;
u32 pen_ps_index = 0;
u32 pen_pe_index = 0;
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].p;
struct v2 pe = proto[pe_index].p;
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
f32 w = v2_wedge(vse, velocity);
if (w != 0) {
w = 1 / w;
f32 t1 = v2_wedge(vso, velocity) * w;
f32 t2 = v2_wedge(vso, vse) * w;
(UNUSED)t1;
(UNUSED)t2;
b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1;
if (t2 > furthest) {
pen_ps_index = ps_index;
pen_pe_index = pe_index;
furthest = t2;
dir = v2_perp_towards_dir(vse, velocity);
//dir = velocity;
}
}
}
#else
f32 highest_score = -F32_INFINITY;
u32 pen_ps_index = 0;
u32 pen_pe_index = 0;
u32 picked_start_index = 0;
u32 picked_end_index = 0;
b32 picked_intersects = false;
f32 picked_intersection = 0;
for (u32 i = 0; i < proto_count; ++i) {
u32 ps_index = i;
u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
@ -679,13 +575,13 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
struct v2 vse = v2_sub(pe, ps);
struct v2 vso = v2_neg(ps);
f32 w = v2_wedge(vse, velocity);
f32 w = v2_wedge(vse, linear_velocity);
f32 t1 = 0;
f32 t2 = 0;
b32 intersects = false;
if (w != 0) {
w = 1 / w;
t1 = v2_wedge(vso, velocity) * w;
t1 = v2_wedge(vso, linear_velocity) * w;
t2 = v2_wedge(vso, vse) * w;
intersects = -epsilon < t1 && t1 < 1 + epsilon && -epsilon < t2 && t2 < 1 + epsilon;
}
@ -695,10 +591,10 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
if (intersects) {
/* Score is intersection (+1 to ensure intersections get priority) */
score = 1 + t1;
next_dir = v2_perp_towards_dir(vse, velocity);
next_dir = v2_perp_towards_dir(vse, linear_velocity);
} else {
/* Score is inverse projection len */
struct v2 vsv = v2_sub(velocity, ps);
struct v2 vsv = v2_sub(linear_velocity, ps);
struct v2 vsd = v2_mul(vse, v2_dot(vse, vsv) / v2_dot(vse, vse));
f32 len = v2_len(v2_sub(vsd, vsv));
score = 1.0 - len;
@ -706,21 +602,24 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
}
if (score > highest_score) {
pen_ps_index = ps_index;
pen_pe_index = pe_index;
picked_start_index = ps_index;
picked_end_index = pe_index;
highest_score = score;
picked_intersects = intersects;
picked_intersection = t1;
dir = next_dir;
}
}
#endif
/* TODO: Move to break (debugging) */
s.a = proto[pen_ps_index];
s.b = proto[pen_pe_index];
velocity_intersects = picked_intersects;
velocity_intersection = picked_intersection;
s.a = proto[picked_start_index];
s.b = proto[picked_end_index];
s.len = 2;
/* Find new point in dir */
m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1);
m = menkowski_point_extended_test(shape0, shape1, dir, linear_velocity);
/* Check unique */
/* TODO: Better */
@ -778,39 +677,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
}
}
#if 0
/* Resolve points */
if (s.len == 1) {
shape0_p = s.a.p0;
shape1_p = s.a.p1;
} else if (s.len == 2) {
/* FIXME: Winding order dependent? */
ASSERT(s.len == 2);
f32 ratio;
{
/* Determine ratio between edge a & b that projected origin lies */
struct v2 vab = v2_sub(s.b.p, s.a.p);
struct v2 vao = v2_neg(s.a.p);
ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
}
/* Shape 0 */
shape0_p = v2_sub(s.b.p0, s.a.p0);
shape0_p = v2_mul(shape0_p, ratio);
shape0_p = v2_add(shape0_p, s.a.p0);
/* Shape 1 */
shape1_p = v2_sub(s.b.p1, s.a.p1);
shape1_p = v2_mul(shape1_p, ratio);
shape1_p = v2_add(shape1_p, s.a.p1);
}
#else
if (s.len == 1) {
shape0_p = s.a.p0;
shape1_p = s.a.p1;
@ -818,29 +685,16 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
ASSERT(s.len == 2);
/* FIXME: Winding order dependent? */
f32 ratio;
if (use_penetration_dir) {
struct v2 vao = v2_neg(s.a.p);
struct v2 vab = v2_sub(s.b.p, s.a.p);
f32 w = 1 / v2_wedge(vab, velocity);
f32 t1 = v2_wedge(vao, velocity) * w;
f32 t2 = v2_wedge(vao, vab) * w;
velocity_intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1;
#if 1
if (colliding && !v2_eq(linear_velocity, V2(0, 0))) {
if (velocity_intersects) {
/* Ratio between edge a & b that velocity intersection lies */
ratio = t1;
ratio = velocity_intersection;
} else {
/* Ratio between edge a & b that projected velocity lies */
struct v2 vap = v2_sub(velocity, s.a.p);
struct v2 vab = v2_sub(s.b.p, s.a.p);
struct v2 vap = v2_sub(linear_velocity, s.a.p);
ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1);
}
#else
/* Ratio between edge a & b that velocity intersection lies */
ratio = t1;
#endif
} else {
/* Determine ratio between edge a & b that projected origin lies */
struct v2 vab = v2_sub(s.b.p, s.a.p);
@ -857,8 +711,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
shape1_p = v2_add(shape1_p, s.a.p1);
}
#endif
res.solved = true;
abort:
@ -890,14 +742,14 @@ abort:
/* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0 ,struct xform xf1)
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
{
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
u64 rays = 500;
for (u64 i = 0; i < rays; ++i) {
f32 angle = ((f32)i / rays) * (2 * PI);
struct v2 dir = v2_from_angle(angle);
struct v2 p = menkowski_point_extended_test(poly0, poly1, dir, xf0, xf1).p;
struct v2 p = menkowski_point_extended_test(poly0, poly1, dir, linear_velocity).p;
if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
*arena_push(arena, struct v2) = p;
++res.count;
@ -906,14 +758,10 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_
return res;
}
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1)
/* TODO: Remove this (debugging) */
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
{
(UNUSED)xf0;
(UNUSED)xf1;
struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
struct v2 velocity = v2_sub(xf1.og, xf0.og);
for (u64 i = 0; i < poly0.count; ++i) {
struct v2 p0 = poly0.points[i];
for (u64 j = 0; j < poly1.count; ++j) {
@ -924,7 +772,7 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra
++res.count;
}
{
*arena_push(arena, struct v2) = v2_add(diff, velocity);;
*arena_push(arena, struct v2) = v2_add(diff, linear_velocity);
++res.count;
}
}

View File

@ -62,11 +62,11 @@ struct gjk_extended_result {
struct gjk_prototype prototype;
};
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1);
struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity);
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1);
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity);
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1);
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity);
#endif

View File

@ -1057,7 +1057,7 @@ INTERNAL void user_update(void)
//struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly);
struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1);
struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og));
for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]);
draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color);
@ -1071,7 +1071,7 @@ INTERNAL void user_update(void)
//struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly);
struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1);
struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, v2_sub(ent->xf1.og, ent->xf0.og));
for (u64 i = 0; i < m.count; ++i) {
struct v2 p = xform_mul_v2(G.world_view, m.points[i]);;
@ -1203,8 +1203,8 @@ INTERNAL void user_update(void)
/* Queue player move cmd */
f32 move_speed = 1.0f;
if (G.bind_states[USER_BIND_KIND_WALK].is_held) {
//const f32 walk_ratio = 0.25f;
const f32 walk_ratio = 0.01f;
const f32 walk_ratio = 0.25f;
//const f32 walk_ratio = 0.01f;
move_speed *= walk_ratio;
}