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

gjk tweaks

Browse Source
This commit is contained in:
jacob 2024-08-30 09:00:18 -05:00
parent 8de566cad5
commit 99381f1274
4 changed files with 17 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/math.h
View File

@ -651,9 +651,9 @@ INLINE struct v2 v2_norm(struct v2 a)
{
f32 l = v2_len_squared(a);
if (l != 0) {
l = math_sqrt(l);
a.x /= l;
a.y /= l;
f32 denom = 1.f / math_sqrt(l);
a.x *= denom;
a.y *= denom;
}
return a;
}

8
src/user.c
View File

@ -1034,11 +1034,11 @@ INTERNAL void user_update(void)
/* Draw simplex */
{
f32 thickness = 2;
f32 thickness = 5;
u32 line_color = colliding ? COLOR_WHITE: COLOR_YELLOW;
u32 color_first = COLOR_RED;
u32 color_second = COLOR_GREEN;
u32 color_third = COLOR_TURQOISE;
u32 color_first = RGBA_32_F(1, 0, 0, 0.5);
u32 color_second = RGBA_32_F(0, 1, 0, 0.5);
u32 color_third = RGBA_32_F(0, 0, 1, 0.5);
struct simplex simplex = ent->simplex;
struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c };

181
src/util.c
View File

@ -65,158 +65,13 @@ struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v
return v2_sub(poly_support(poly0, dir), poly_support(poly1, v2_neg(dir)));
}
struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p)
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p)
{
struct v2 dir = v2_norm(v2_perp_cw(v2_sub(end, start)));
i32 sign = 1 - ((v2_dot(dir, v2_sub(p, start)) < 0) << 1);
return v2_mul(dir, sign);
struct v2 perp = v2_perp_cw(v2_sub(end, start));
i32 sign = 1 - ((v2_dot(perp, v2_sub(p, start)) < 0) << 1);
return v2_mul(perp, sign);
}
#if 0
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
{
struct v2 poly0_center = math_poly_center(poly0);
struct v2 poly1_center = math_poly_center(poly1);
/* Simplex */
struct simplex s = { 0 };
/* Append first point to simplex */
struct v2 dir = v2_norm(v2_sub(poly1_center, poly0_center));
s.a = menkowski_point(poly0, poly1, dir);
s.len = 1;
dir = v2_norm(v2_neg(s.a)); /* Next point is towards origin */
b32 colliding = false;
while (true) {
/* Determine support point */
struct v2 p = menkowski_point(poly0, poly1, dir);
if (v2_dot(dir, p) < 0) {
/* Point did not cross origin */
colliding = false;
break;
}
if (s.len < 3) {
/* Line case */
/* Next dir is line normal towards origin */
if (s.len == 1) {
s.b = s.a;
s.a = p;
} else if (s.len == 2) {
s.c = s.b;
s.b = s.a;
s.a = p;
}
++s.len;
dir = normal_towards_point(s.a, s.b, V2(0, 0));
} else {
/* Triangle case */
s.c = s.b;
s.b = s.a;
s.a = p;
/* Ensure new point is unique */
if (v2_eq(s.a, s.b) || v2_eq(s.b, s.c) || v2_eq(s.a, s.c)) {
colliding = false;
break;
}
struct v2 a_to_origin_rel = v2_neg(s.a);
dir = v2_neg(normal_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
if (v2_dot(dir, a_to_origin_rel) >= 0) {
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
} else {
dir = v2_neg(normal_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
if (v2_dot(dir, a_to_origin_rel) >= 0) {
/* Point is in region ac, remove b from simplex */
s.b = s.c;
} else {
/* Point is in simplex */
colliding = true;
break;
}
}
}
}
return (struct gjk_result) {
.colliding = colliding,
.final_simplex = s
};
}
#elif 0
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1)
{
b32 colliding = false;
/* Simplex */
struct simplex s = { 0 };
/* First point is support point towards shape centers */
{
s.a = menkowski_point(poly0, poly1, v2_norm(v2_sub(poly1.points[0], poly0.points[0])));
s.len = 1;
}
/* Second point is support point towards origin */
{
struct v2 dir = v2_norm(v2_neg(s.a));
struct v2 p = menkowski_point(poly0, poly1, dir);
if (!v2_eq(p, s.a)) {
s.b = s.a;
s.a = p;
s.len = 2;
}
}
if (s.len == 2) {
while (true) {
/* Third point is support point in direction of line normal towards origin */
struct v2 dir = normal_towards_point(s.a, s.b, V2(0, 0));
struct v2 p = menkowski_point(poly0, poly1, dir);
if (v2_eq(p, s.a) || v2_eq(p, s.b)) {
colliding = false;
break;
}
s.c = s.b;
s.b = s.a;
s.a = p;
struct v2 a_to_origin = v2_neg(s.a);
dir = v2_neg(normal_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
} else {
/* Point is not in region ab */
dir = v2_neg(normal_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ac, remove b from simplex */
s.b = s.c;
} else {
/* Point is in simplex */
s.len = 3;
colliding = true;
break;
}
}
}
}
return (struct gjk_result) {
.colliding = colliding,
.final_simplex = s
};
}
#else
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps)
{
b32 colliding = false;
@ -235,21 +90,13 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
/* Second point is support point towards origin */
if (step++ >= max_steps) goto abort;
{
struct v2 dir = v2_norm(v2_neg(s.a));
struct v2 dir = v2_neg(s.a);
struct v2 p = menkowski_point(poly0, poly1, dir);
#if 0
if (!v2_eq(p, s.a)) {
if (v2_dot(dir, p) >= 0) {
s.b = s.a;
s.a = p;
s.len = 2;
}
#else
if (v2_dot(s.a, p) < 0) {
s.b = s.a;
s.a = p;
s.len = 2;
}
#endif
}
if (s.len == 2) {
@ -257,33 +104,24 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
if (step++ >= max_steps) goto abort;
/* Third point is support point in direction of line normal towards origin */
struct v2 dir = normal_towards_point(s.a, s.b, V2(0, 0));
struct v2 dir = perp_towards_point(s.a, s.b, V2(0, 0));
struct v2 p = menkowski_point(poly0, poly1, dir);
#if 0
if (v2_eq(p, s.a) || v2_eq(p, s.b)) {
colliding = false;
break;
}
#else
if (v2_dot(dir, p) < 0) {
colliding = false;
break;
}
#endif
s.c = s.b;
s.b = s.a;
s.a = p;
dir = v2_neg(perp_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
struct v2 a_to_origin = v2_neg(s.a);
dir = v2_neg(normal_towards_point(s.a, s.b, s.c)); /* Normal dir of ab pointing away from c */
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
} else {
/* Point is not in region ab */
dir = v2_neg(normal_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
dir = v2_neg(perp_towards_point(s.a, s.c, s.b)); /* Normal dir of ac pointing away from b */
if (v2_dot(dir, a_to_origin) >= 0) {
/* Point is in region ac, remove b from simplex */
s.b = s.c;
@ -304,7 +142,6 @@ struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_step
};
}
#endif
struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex)
{

2
src/util.h
View File

@ -198,7 +198,7 @@ struct gjk_result {
struct v2 poly_support(struct v2_array a, struct v2 dir);
struct v2 menkowski_point(struct v2_array poly0, struct v2_array poly1, struct v2 dir);
struct v2 normal_towards_point(struct v2 start, struct v2 end, struct v2 p);
struct v2 perp_towards_point(struct v2 start, struct v2 end, struct v2 p);
struct gjk_result gjk(struct v2_array poly0, struct v2_array poly1, u32 max_steps);
struct v2 epa(struct v2_array poly0, struct v2_array poly1, struct simplex simplex);
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1);