return un-negated normal from gjk
This commit is contained in:
parent
acbd97aa84
commit
62df86a66a
@ -246,7 +246,7 @@ void draw_solid_arrow_line(struct renderer_canvas *canvas, struct v2 start, stru
|
||||
|
||||
struct v2 head_start = v2_add(end, head_start_dir);
|
||||
|
||||
struct v2 head_p1_dir = v2_perp_right_mul(head_start_dir, head_width_ratio);
|
||||
struct v2 head_p1_dir = v2_perp_mul(head_start_dir, head_width_ratio);
|
||||
struct v2 head_p2_dir = v2_neg(head_p1_dir);
|
||||
|
||||
struct v2 head_p1 = v2_add(head_start, head_p1_dir);
|
||||
|
||||
81
src/game.c
81
src/game.c
@ -133,10 +133,11 @@ INTERNAL void spawn_test_entities(void)
|
||||
//struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */
|
||||
//struct v2 size = V2(1, 1);
|
||||
struct v2 size = V2(0.5, 0.5);
|
||||
f32 r = PI;
|
||||
//f32 r = PI / 4;
|
||||
//f32 r = PI / 3;
|
||||
//f32 r = PI / 2;
|
||||
f32 r = 0;
|
||||
//f32 r = 0;
|
||||
f32 skew = 0;
|
||||
|
||||
struct entity *e = entity_alloc(root);
|
||||
@ -160,8 +161,8 @@ INTERNAL void spawn_test_entities(void)
|
||||
|
||||
entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
|
||||
e->mass_unscaled = 100;
|
||||
e->inertia_unscaled = 25;
|
||||
//e->inertia_unscaled = F32_INFINITY;
|
||||
//e->inertia_unscaled = 25;
|
||||
e->inertia_unscaled = F32_INFINITY;
|
||||
e->linear_ground_friction = 1000;
|
||||
e->angular_ground_friction = 100;
|
||||
|
||||
@ -196,9 +197,11 @@ INTERNAL void spawn_test_entities(void)
|
||||
if (!G.box_spawned) {
|
||||
G.box_spawned = true;
|
||||
|
||||
struct v2 pos = V2(0.5, -1);
|
||||
//struct v2 pos = V2(0.5, -1);
|
||||
struct v2 pos = V2(0.5, 20);
|
||||
//struct v2 pos = V2(1, -1);
|
||||
struct v2 size = V2(1, 1);
|
||||
//struct v2 size = V2(1, 1);
|
||||
struct v2 size = V2(50, 50);
|
||||
//f32 rot = PI / 4;
|
||||
f32 rot = 0;
|
||||
struct entity *e = entity_alloc(root);
|
||||
@ -510,6 +513,11 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
|
||||
struct entity *e0 = entity_from_handle(store, manifold->manifold_e0);
|
||||
struct entity *e1 = entity_from_handle(store, manifold->manifold_e1);
|
||||
|
||||
struct v2 v0 = e0->linear_velocity;
|
||||
f32 w0 = e0->angular_velocity;
|
||||
struct v2 v1 = e1->linear_velocity;
|
||||
f32 w1 = e1->angular_velocity;
|
||||
|
||||
u32 num_contacts = manifold->num_contacts;
|
||||
f32 inv_num_contacts = 1.f / num_contacts;
|
||||
(UNUSED)inv_num_contacts;
|
||||
@ -528,70 +536,54 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
|
||||
f32 inv_i0 = 1.f / i0;
|
||||
f32 inv_i1 = 1.f / i1;
|
||||
|
||||
struct queued_impulse {
|
||||
struct v2 p0, p1;
|
||||
f32 impulse;
|
||||
};
|
||||
struct queued_impulse queued_impulses[ARRAY_COUNT(manifold->contacts)];
|
||||
|
||||
struct v2 normal = manifold->manifold_normal;
|
||||
|
||||
for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) {
|
||||
struct contact *contact = &manifold->contacts[contact_index];
|
||||
|
||||
struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0);
|
||||
struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1);
|
||||
f32 separation = v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation;
|
||||
|
||||
f32 bias = 0;
|
||||
if (apply_bias) {
|
||||
//f32 bias_factor = 0.2f;
|
||||
f32 bias_factor = 0.1f;
|
||||
//f32 bias_factor = 0.025f;
|
||||
|
||||
//f32 bias_slop = 0.0005f;
|
||||
f32 bias_slop = 0.005f;
|
||||
//f32 bias_slop = 0.00f;
|
||||
|
||||
bias = (bias_factor / dt) * max_f32((separation - bias_slop), 0);
|
||||
const f32 bias_factor = 0.2f;
|
||||
const f32 bias_slop = 0.005f;
|
||||
f32 penetration = v2_dot(v2_sub(p0, p1), normal) - contact->starting_separation;
|
||||
bias = (bias_factor / dt) * max_f32(penetration - bias_slop, 0);
|
||||
}
|
||||
|
||||
struct v2 vcp0 = v2_sub(p0, e0_xf.og);
|
||||
struct v2 vcp1 = v2_sub(p1, e1_xf.og);
|
||||
|
||||
struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_right_mul(vcp0, e0->angular_velocity));
|
||||
struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_right_mul(vcp1, e1->angular_velocity));
|
||||
|
||||
struct v2 vrel = v2_sub(vel1, vel0);
|
||||
|
||||
f32 vn = v2_dot(vrel, normal);
|
||||
|
||||
struct v2 idk0 = v2_perp_right_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
|
||||
struct v2 idk1 = v2_perp_right_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
|
||||
struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity));
|
||||
struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity));
|
||||
struct v2 vrel = v2_sub(vel0, vel1);
|
||||
|
||||
struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
|
||||
struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
|
||||
f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1));
|
||||
|
||||
/* (to be applied along n) */
|
||||
f32 vn = v2_dot(vrel, normal);
|
||||
f32 j = (vn + bias) / k;
|
||||
j *= inv_num_contacts; /* TODO: Is this the correct place to do this? */
|
||||
j *= inv_num_contacts;
|
||||
|
||||
f32 old_accumulated_impulse = contact->accumulated_impulse;
|
||||
f32 new_accumulated_impulse = max_f32(contact->accumulated_impulse + j, 0);
|
||||
f32 delta_impulse = new_accumulated_impulse - old_accumulated_impulse;
|
||||
f32 delta = new_accumulated_impulse - old_accumulated_impulse;
|
||||
contact->accumulated_impulse = new_accumulated_impulse;
|
||||
|
||||
queued_impulses[contact_index] = (struct queued_impulse) {
|
||||
.p0 = p0,
|
||||
.p1 = p1,
|
||||
.impulse = delta_impulse
|
||||
};
|
||||
struct v2 impulse = v2_mul(normal, delta);
|
||||
v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
|
||||
w0 -= v2_wedge(vcp0, impulse) * inv_i0;
|
||||
v1 = v2_add(v1, v2_mul(impulse, inv_m1));
|
||||
w1 += v2_wedge(vcp1, impulse) * inv_i1;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < num_contacts; ++i) {
|
||||
struct queued_impulse qi = queued_impulses[i];
|
||||
struct v2 impulse = v2_mul(normal, qi.impulse);
|
||||
entity_apply_linear_impulse(e0, impulse, qi.p0);
|
||||
entity_apply_linear_impulse(e1, v2_neg(impulse), qi.p1);
|
||||
}
|
||||
e0->linear_velocity = v0;
|
||||
e0->angular_velocity = w0;
|
||||
e1->linear_velocity = v1;
|
||||
e1->angular_velocity = w1;
|
||||
}
|
||||
}
|
||||
#else
|
||||
@ -1304,7 +1296,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
|
||||
integrate_positions(substep_dt);
|
||||
solve_collisions(substep_dt, false); /* Relaxation */
|
||||
#else
|
||||
solve_collisions(substep_dt, true);
|
||||
//solve_collisions(substep_dt, true);
|
||||
solve_collisions(substep_dt, false);
|
||||
integrate_positions(substep_dt);
|
||||
#endif
|
||||
}
|
||||
|
||||
14
src/gjk.c
14
src/gjk.c
@ -310,9 +310,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
|
||||
struct v2 a = proto[pen_ps_index];
|
||||
struct v2 b = proto[pen_pe_index];
|
||||
struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */
|
||||
struct v2 van = v2_sub(n, a);
|
||||
struct v2 vab = v2_sub(b, a);
|
||||
dir = v2_perp_towards_dir(vab, v2_neg(van));
|
||||
struct v2 vna = v2_sub(a, n);
|
||||
dir = v2_perp_towards_dir(vab, vna);
|
||||
}
|
||||
m = menkowski_point(shape0, shape1, dir);
|
||||
|
||||
@ -504,16 +504,16 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
|
||||
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);
|
||||
f32 a_sep = v2_dot(va0a1_clipped, normal);
|
||||
f32 b_sep = v2_dot(vb0b1_clipped, normal);
|
||||
|
||||
if (a_sep > 0) {
|
||||
if (a_sep < 0) {
|
||||
struct gjk_contact_point *point = &points[num_points++];
|
||||
point->id = id_a0 | (id_a1 << 4);
|
||||
point->separation = a_sep;
|
||||
point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
|
||||
}
|
||||
if (b_sep > 0) {
|
||||
if (b_sep < 0) {
|
||||
struct gjk_contact_point *point = &points[num_points++];
|
||||
point->id = id_b0 | (id_b1 << 4);
|
||||
point->separation = b_sep;
|
||||
@ -545,7 +545,7 @@ abort:
|
||||
}
|
||||
res.prototype.len = s.len;
|
||||
}
|
||||
res.normal = v2_neg(normal);
|
||||
res.normal = normal;
|
||||
res.points[0] = points[0];
|
||||
res.points[1] = points[1];
|
||||
res.num_points = num_points;
|
||||
|
||||
26
src/math.h
26
src/math.h
@ -648,41 +648,29 @@ INLINE f32 v2_wedge(struct v2 a, struct v2 b)
|
||||
return a.x * b.y - a.y * b.x;
|
||||
}
|
||||
|
||||
/* Clockwise perpendicular vector */
|
||||
INLINE struct v2 v2_perp_right(struct v2 a)
|
||||
/* Clockwise (right) perpendicular vector */
|
||||
INLINE struct v2 v2_perp(struct v2 a)
|
||||
{
|
||||
return V2(-a.y, a.x);
|
||||
}
|
||||
|
||||
/* Counter-clockwise perpendicular vector */
|
||||
INLINE struct v2 v2_perp_left(struct v2 a)
|
||||
{
|
||||
return V2(a.y, -a.x);
|
||||
}
|
||||
|
||||
/* Clockwise perpendicular vector scaled by s */
|
||||
INLINE struct v2 v2_perp_right_mul(struct v2 a, f32 s)
|
||||
/* Clockwise (right) perpendicular vector scaled by s */
|
||||
INLINE struct v2 v2_perp_mul(struct v2 a, f32 s)
|
||||
{
|
||||
return V2(s * -a.y, s * a.x);
|
||||
}
|
||||
|
||||
/* Counter-clockwise perpendicular vector scaled by s */
|
||||
INLINE struct v2 v2_perp_left_mul(struct v2 a, f32 s)
|
||||
{
|
||||
return V2(s * a.y, s * -a.x);
|
||||
}
|
||||
|
||||
INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir)
|
||||
{
|
||||
f32 wedge = v2_wedge(v, dir);
|
||||
return v2_perp_right_mul(v, (wedge >= 0) - (wedge < 0));
|
||||
return v2_perp_mul(v, (wedge >= 0) - (wedge < 0));
|
||||
}
|
||||
|
||||
INLINE struct v2 v2_norm(struct v2 a)
|
||||
{
|
||||
f32 l = a.x * a.x + a.y * a.y;
|
||||
if (l != 0) {
|
||||
/* TODO: Benchmark with math_rqsrt(l) */
|
||||
/* TODO: Benchmark vs math_rqsrt(l) */
|
||||
f32 denom = 1.f / math_sqrt(l);
|
||||
a.x *= denom;
|
||||
a.y *= denom;
|
||||
@ -1100,7 +1088,7 @@ INLINE struct quad quad_from_line(struct v2 start, struct v2 end, f32 thickness)
|
||||
f32 width = thickness / 2.f;
|
||||
|
||||
struct v2 dir = v2_norm(v2_sub(end, start));
|
||||
struct v2 dir_perp = v2_perp_right(dir);
|
||||
struct v2 dir_perp = v2_perp(dir);
|
||||
|
||||
struct v2 left = v2_mul(dir_perp, -width);
|
||||
struct v2 right = v2_mul(dir_perp, width);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user