Skip to content

Instantly share code, notes, and snippets.

@stillwwater

stillwwater/gjk.c

Last active September 2, 2023 22:27
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save stillwwater/b54b8ff655395b86c7e9c4a1fc0415c3 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save stillwwater/b54b8ff655395b86c7e9c4a1fc0415c3 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gjk.c
#include "core/vector.h"
#include "core/math.h"
#include "physics/clip.h"
struct phys_world {
unsigned static_clips_count;
struct aabb static_clips[PHYS_MAX_CLIP];
};
static struct phys_world phys;
// Returns the min and max points describing an AABB.
void
aabb_bounds(float minb[3], float maxb[3], struct aabb *clip)
{
vec3_sub(minb, clip->center, clip->radius);
vec3_add(maxb, clip->center, clip->radius);
}
// Returns an identifier for the newly created clip.
unsigned
phys_make_static_clip(struct aabb clip)
{
unsigned id = phys.static_clips_count;
phys.static_clips[phys.static_clips_count] = clip;
++phys.static_clips_count;
return id;
}
void
aabb_support(float support[4], struct aabb *aabb, float direction[4])
{
support[0] = direction[0] > 0 ? aabb->radius[0] : -aabb->radius[0];
support[1] = direction[1] > 0 ? aabb->radius[1] : -aabb->radius[1];
support[2] = direction[2] > 0 ? aabb->radius[2] : -aabb->radius[2];
vec3_add(support, support, aabb->center);
//printf("(%f, %f, %f)\n", (double)support[0], (double)support[1], (double)support[2]);
}
/*
void
hull_support(float support[4], struct hull *hull, float direction[4])
{
float maxv = -INFINITY;
unsigned maxi = 0;
for (unsigned i = 0; i < hull->vertices_count; ++i) {
float v = vec3_dot(hull->vertices[i], direction);
if (v > maxv) {
maxv = v;
maxi = i;
}
}
vec3_copy(support, hull->vertices[maxi]);
}
*/
// depth:
// The current best separation axis which separates the two boxes.
//
// min_dist:
// The current minimum squared distance between the two boxes.
//
// axis:
// Axis to test against, must be a unit vector.
static bool
sat_test_axis(float depth[3], float *min_dist, float axis[3],
float amin, float amax, float bmin, float bmax)
{
float separation_axis[3];
float d0 = bmax - amin;
float d1 = amax - bmin;
// No intersection
if (d0 <= 0 || d1 <= 0) return 0;
float penetration = (d0 < d1) ? d0 : -d1;
vec3_muls(separation_axis, axis, penetration);
float separation_length_sqr = vec3_dot(separation_axis, separation_axis);
if (separation_length_sqr < *min_dist) {
*min_dist = separation_length_sqr;
vec3_copy(depth, separation_axis);
}
return 1;
}
// Tests intersection between two stationary AABBs using SAT. Returns the
// penetration depth that describes the overlap between the two boxes. If no
// intersection occurs, the penetration depth is unmodified.
bool
phys_intersect_aabb_sat(float penetration_depth[3], struct aabb *a, struct aabb *b)
{
float right[3] = {1, 0, 0};
float forward[3] = {0, 1, 0};
float up[3] = {0, 0, 1};
float amin[3], amax[3];
float bmin[3], bmax[3];
float min_depth = INFINITY;
float pd[3] = {0};
aabb_bounds(amin, amax, a);
aabb_bounds(bmin, bmax, b);
if (!sat_test_axis(pd, &min_depth, right, amin[0], amax[0], bmin[0], bmax[0])) {
return 0;
}
if (!sat_test_axis(pd, &min_depth, forward, amin[1], amax[1], bmin[1], bmax[1])) {
return 0;
}
if (!sat_test_axis(pd, &min_depth, up, amin[2], amax[2], bmin[2], bmax[2])) {
return 0;
}
vec3_copy(penetration_depth, pd);
return 1;
}
// clip:
// The AABB clip to test the ray against.
static struct ray_hit
ray_intersect_aabb(struct aabb *clip, float ray_origin[3], float ray_direction[3])
{
struct ray_hit hit = {0};
float minb[3], maxb[3];
float tmin, tmax;
aabb_bounds(minb, maxb, clip);
tmin = -INFINITY;
tmax = INFINITY;
hit.t = INFINITY;
for (unsigned i = 0; i < 3; ++i) {
if (ray_direction[i]) {
float inv_d = 1.0f / ray_direction[i];
float t0 = (minb[i] - ray_origin[i]) * inv_d;
float t1 = (maxb[i] - ray_origin[i]) * inv_d;
tmin = fmaxf(tmin, fminf(t0, t1));
tmax = fminf(tmax, fmaxf(t0, t1));
} else if (ray_origin[i] <= minb[i] || ray_origin[i] >= maxb[i]) {
return hit;
}
}
if (tmax > tmin && tmax > 0 && tmin < 1) {
vec3_muls(hit.position, ray_direction, tmin);
vec3_add(hit.position, hit.position, ray_origin);
// Construct normal at the AABB surface
float delta[3], abs_delta[3];
vec3_sub(delta, hit.position, clip->center);
vec3_normalize(delta);
vec3_abs(abs_delta, delta);
// The largest axis of the delta is set to 1 (or -1) for the normal
// vector. All other axes are set to 0.
if (abs_delta[0] > abs_delta[1] && abs_delta[0] > abs_delta[2]) {
hit.normal[0] = copysignf(1, delta[0]);
} else if (abs_delta[1] > abs_delta[2]) {
hit.normal[1] = copysignf(1, delta[1]);
} else {
hit.normal[2] = copysignf(1, delta[2]);
}
hit.t = tmin;
}
return hit;
}
// Tests a raycast against all static entities. The ray source is assumed to
// come from an AABB which will be sweeped around the static entities tested.
//
// ray_origin:
// The AABB to test against all other AABBs.
//
// ray_direction:
// Direction and magnitude of the ray.
//
// Returns the position of the nearest hit, the normal of the surface at the
// hit point and the time `t` in which the hit occurs [0, 1). If `t` is less
// than 1 then a hit occured, otherwise there was no hit.
struct ray_hit
phys_raycast_clip(struct aabb *ray_origin, float ray_direction[3])
{
struct ray_hit nearest_hit = {.t = INFINITY};
for (unsigned i = 0; i < phys.static_clips_count; ++i) {
struct aabb *static_clip = &phys.static_clips[i];
struct aabb sum;
// Minkowski sum converting the ray origin clip to a single point
vec3_copy(sum.center, static_clip->center);
vec3_add(sum.radius, ray_origin->radius, static_clip->radius);
// Test ray against swept AABB
struct ray_hit hit = ray_intersect_aabb(&sum, ray_origin->center, ray_direction);
if (hit.t < nearest_hit.t) {
nearest_hit = hit;
}
}
return nearest_hit;
}
static void
closest_point_line(float v[3], char simplex[4], float P[4][3])
{
float ab[3];
float ao[3];
float *a = P[0];
float *b = P[1];
vec3_sub(ab, b, a);
vec3_inverse(ao, a);
float len2 = vec3_dot(ab, ab);
float t = vec3_dot(ao, ab);
if (len2 < FLT_EPSILON * FLT_EPSILON) {
// Degenerate: check which point is closest to the origin
if (vec3_dot(a, a) < vec3_dot(b, b)) {
// A is the closest point
vec3_copy(v, a);
simplex[0] = 1;
return;
}
// B is the closest point
vec3_copy(v, b);
simplex[1] = 1;
return;
}
if (t <= 0) {
// A is the closest point
vec3_copy(v, a);
simplex[0] = 1;
return;
}
if (t >= len2) {
// B is the closest point
vec3_copy(v, b);
simplex[1] = 1;
return;
}
// Closest points lies on line AB, interpolate beween A, B
t = t / len2;
vec3_muls(v, ab, t);
vec3_add(v, v, a);
simplex[0] = 1;
simplex[1] = 1;
}
// Returns the closest point `v` to the origin and a new simplex `Y` with `n`
// points given a triangle defined by points ABC in P.
//
// Reference: Real-Time Collision Detection - Christer Ericson (chapter 5.1)
static void
closest_point_triangle(float v[3], char simplex[4], float P[4][3])
{
float ab[3]; // B - A
float ac[3]; // C - A
float bc[3]; // C - B
float ao[3]; // Origin - A
float bo[3]; // Origin - B
float co[3]; // Origin - C
float abc[3]; // AB x AC
float *a = P[0];
float *b = P[1];
float *c = P[2];
vec3_sub(ac, c, a); // Edge AC
vec3_sub(bc, c, b); // Edge BC
/*
// Use the two shortest edges to improve accuracy of the triangle normal.
// Swap A,C if necessary.
if (vec3_dot(bc, bc) < vec3_dot(ac, ac)) {
a = P[2];
c = P[0];
vec3_sub(ac, c, a); // Edge AC (again)
// Edge BC does not need to be recomputed because it is not used any further
}
*/
vec3_sub(ab, b, a); // Edge AB
vec3_cross(abc, ab, ac); // Face normal
if (vec3_dot(abc, abc) < FLT_EPSILON * FLT_EPSILON) {
// Degenerate ;-;
float closest[3];
float best_distance;
float distance;
// Start with C being the closest
simplex[2] = 1;
vec3_copy(closest, c);
best_distance = vec3_dot(closest, closest);
// Vertex A
distance = vec3_dot(a, a);
if (distance < best_distance) {
simplex[0] = 1;
simplex[1] = 0;
simplex[2] = 0;
vec3_copy(closest, a);
best_distance = distance;
}
// Vertex B
distance = vec3_dot(b, b);
if (distance < best_distance) {
simplex[0] = 0;
simplex[1] = 1;
simplex[2] = 0;
vec3_copy(closest, b);
best_distance = distance;
}
// Edge AB
distance = vec3_dot(ab, ab);
if (distance > FLT_EPSILON * FLT_EPSILON) {
float q[3];
float vx = clamp(-vec3_dot(a, ab) / distance, 0.0f, 1.0f);
vec3_muls(q, ab, vx);
vec3_add(q, q, a);
distance = vec3_dot(q, q);
if (distance < best_distance) {
simplex[0] = 1;
simplex[1] = 1;
simplex[2] = 0;
vec3_copy(closest, q);
best_distance = distance;
}
}
// Edge AC
distance = vec3_dot(ac, ac);
if (distance > FLT_EPSILON * FLT_EPSILON) {
float q[3];
float vx = clamp(-vec3_dot(a, ac) / distance, 0.0f, 1.0f);
vec3_muls(q, ac, vx);
vec3_add(q, q, a);
distance = vec3_dot(q, q);
if (distance < best_distance) {
simplex[0] = 1;
simplex[1] = 0;
simplex[2] = 1;
vec3_copy(closest, q);
best_distance = distance;
}
}
// Edge AC
distance = vec3_dot(bc, bc);
if (distance > FLT_EPSILON * FLT_EPSILON) {
float q[3];
float vx = clamp(-vec3_dot(b, bc) / distance, 0.0f, 1.0f);
vec3_muls(q, bc, vx);
vec3_add(q, q, b);
distance = vec3_dot(q, q);
if (distance < best_distance) {
simplex[0] = 0;
simplex[1] = 1;
simplex[2] = 1;
vec3_copy(closest, q);
best_distance = distance;
}
}
return;
}
// Origin in veronoi region outside vertex A
vec3_inverse(ao, a);
float d1 = vec3_dot(ab, ao);
float d2 = vec3_dot(ac, ao);
if (d1 <= 0 && d2 <= 0) {
vec3_copy(v, a);
simplex[0] = 1;
return;
}
// Origin in veronoi region outside vertex B
vec3_inverse(bo, b);
float d3 = vec3_dot(ab, bo);
float d4 = vec3_dot(ac, bo);
if (d3 >= 0 && d4 <= 0) {
vec3_copy(v, b);
simplex[1] = 1;
return;
}
// Origin in veronoi region outside line AB, project origin onto AB
float vc = d1 * d4 - d3 * d2;
if (vc <= 0 && d1 >= 0 && d3 <= 0) {
float w = d1 / (d1 - d3);
vec3_muls(v, ab, w);
vec3_add(v, v, a);
simplex[0] = 1;
simplex[1] = 1;
return;
}
// Origin in veronoi region outside vertex C
vec3_inverse(co, c);
float d5 = vec3_dot(ab, co);
float d6 = vec3_dot(ac, co);
if (d6 >= 0 && d5 <= d6) {
vec3_copy(v, c);
simplex[2] = 1;
return;
}
// Origin in veronoi region outside line AC, project origin onto AC
float vb = d5 * d2 - d1 * d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
float w = d2 / (d2 - d6);
vec3_muls(v, ab, w);
vec3_add(v, v, a);
simplex[0] = 1;
simplex[2] = 1;
return;
}
// Origin in veronoi region outside line BC, project origin onto BC
float va = d3 * d6 - d5 * d4;
float d4_d3 = d4 - d3;
float d5_d6 = d5 - d6;
if (va <= 0 && d4_d3 >= 0 && d5_d6 >= 0) {
float w = d4_d3 / (d4_d3 + d5_d6);
vec3_sub(v, c, b);
vec3_muls(v, v, w);
vec3_add(v, v, b);
simplex[1] = 1;
simplex[2] = 1;
return;
}
// Triangle is the separating plane, origin may be above or below.
// To improve accuracy of the triangle normal use the distance between the
// triangle centroid and the origin.
// Reference: https://box2d.org/posts/2014/01/troublesome-triangle/
vec3_add(v, b, c);
vec3_add(v, v, a);
float scale = vec3_dot(v, abc) / (3.0f * vec3_dot(abc, abc));
vec3_muls(v, abc, scale);
simplex[0] = 1;
simplex[1] = 1;
simplex[2] = 1;
}
// Returns true if the origin lies outside the triangle plane denoted by ABC.
// `D` determines whether to test the front or back face.
static bool
origin_outside_of_plane(float a[3], float b[3], float c[3], float d[3])
{
float ab[3];
float ac[3];
float ad[3];
float abc[3];
vec3_sub(ab, b, a);
vec3_sub(ac, c, a);
vec3_sub(ad, d, a);
vec3_cross(abc, ab, ac);
float signp = vec3_dot(a, abc); // Using inverse signp to avoid calculating -a
float signd = vec3_dot(ad, abc);
// Points on opposite sides if signs are opposite
return signp * signd > -FLT_EPSILON; // signp * signd > 0
}
static void
closest_point_tetrahedron(float v[3], char simplex[4], float P[4][3])
{
float Q[4][3]; // Triangle for testing without reordering P
// Temporaries for each triangle tested
float q[3];
float q_len;
// Start assuming the origin is inside the tetrahedron
float min_dist = FLT_MAX;
char best_simplex[4] = {1, 1, 1, 1};
// Test for each face of the tetrahedron if the origin is in front of the
// plane, starting with face ABC.
if (origin_outside_of_plane(P[0], P[1], P[2], P[3])) {
puts("ABC");
closest_point_triangle(v, best_simplex, P);
min_dist = vec3_dot(v, v);
}
// Face ACD
if (origin_outside_of_plane(P[0], P[2], P[3], P[1])) {
puts("ACD");
vec3_copy(Q[0], P[0]);
vec3_copy(Q[1], P[2]);
vec3_copy(Q[2], P[3]);
closest_point_triangle(q, simplex, Q);
q_len = vec3_dot(q, q);
if (q_len < min_dist) {
min_dist = q_len;
vec3_copy(v, q);
best_simplex[0] = simplex[0];
best_simplex[1] = 0;
best_simplex[2] = simplex[1];
best_simplex[3] = simplex[2];
}
}
// Face ADB
if (origin_outside_of_plane(P[0], P[3], P[1], P[2])) {
puts("ADB");
// Using back-face ABD to keep vertices in order
vec3_copy(Q[0], P[0]);
vec3_copy(Q[1], P[1]);
vec3_copy(Q[2], P[3]);
closest_point_triangle(q, simplex, Q);
q_len = vec3_dot(q, q);
if (q_len < min_dist) {
min_dist = q_len;
vec3_copy(v, q);
best_simplex[0] = simplex[0];
best_simplex[1] = simplex[1];
best_simplex[2] = 0;
best_simplex[3] = simplex[2];
}
}
// Face BDC
if (origin_outside_of_plane(P[1], P[3], P[2], P[0])) {
puts("BDC");
// Using back-face BCD to keep vertices in order
vec3_copy(Q[0], P[1]);
vec3_copy(Q[1], P[2]);
vec3_copy(Q[2], P[3]);
closest_point_triangle(q, simplex, Q);
q_len = vec3_dot(q, q);
if (q_len < min_dist) {
min_dist = q_len;
vec3_copy(v, q);
best_simplex[0] = 0;
best_simplex[1] = simplex[1];
best_simplex[2] = simplex[2];
best_simplex[3] = simplex[3];
}
}
memcpy(simplex, best_simplex, sizeof(char[4]));
}
static void
barycentric_coords_line(float r[2], float P[4][3])
{
float ab[3]; // Line AB
vec3_sub(ab, P[1], P[0]);
float denom = vec3_dot(ab, ab);
if (denom < FLT_EPSILON * FLT_EPSILON) {
// Degenerate line, use the closest point
if (vec3_dot(P[0], P[0]) < vec3_dot(P[1], P[1])) {
r[0] = 1.0f;
r[1] = 0.0f;
return;
}
// B is the closest point
r[0] = 0.0f;
r[1] = 1.0f;
return;
}
r[0] = -vec3_dot(P[0], ab) / denom;
r[1] = 1.0f - r[0];
}
// Reference: Real-Time Collision Detection - Christer Ericson (chapter 3.4)
static void
barycentric_coords_triangle(float r[3], float P[4][3])
{
float v0[3]; // Edge AB
float v1[3]; // Edge AC
float v2[3]; // Edge BC
vec3_sub(v0, P[1], P[0]); // B - A
vec3_sub(v0, P[2], P[0]); // C - A
vec3_sub(v0, P[2], P[1]); // C - B
// Edge lengths
float d00 = vec3_dot(v0, v0);
float d11 = vec3_dot(v1, v1);
float d22 = vec3_dot(v2, v2);
// Use the shortest edges to produce the result to improve accuracy
if (d00 <= d22) {
// Edges AB, AC
float d01 = vec3_dot(v0, v1);
float denom = d00 * d11 - d01 * d01;
if (fabsf(denom) < FLT_EPSILON * FLT_EPSILON) {
puts("Degenerate BC triangle");
// Degenerate triangle, use longest edge
if (d00 > d11) {
// Edge AB
barycentric_coords_line(r, P);
r[2] = 0.0f;
return;
}
}
float a0 = vec3_dot(P[0], v0);
float a1 = vec3_dot(P[0], v1);
r[1] = (d01 * a1 - d11 * a0) / denom;
r[2] = (d01 * a0 - d00 * a1) / denom;
r[0] = 1.0f - r[1] - r[2];
return;
}
// Edges AC, BC
float d12 = vec3_dot(v1, v2);
float denom = d11 * d22 - d12 * d12;
if (fabsf(denom) < FLT_EPSILON * FLT_EPSILON) {
puts("Degenerate BC triangle");
}
float c1 = vec3_dot(P[2], v1);
float c2 = vec3_dot(P[2], v2);
r[0] = (d22 * c1 - d12 * c2) / denom;
r[1] = (d11 * c2 - d12 * c1) / denom;
r[2] = 1.0f - r[0] - r[1];
}
struct ray_hit
gjk_trace(float ray_origin[3], float ray_direction[3], struct aabb *clip, float t_max)
{
struct ray_hit hit = {0};
unsigned n_iter;
// Simplices being tested
unsigned simplex_count = 0;
float P[4][3];
float Y[4][3];
float x[3]; // Point along the ray
float p[3]; // Support point
float v[3] = {0}; // Support direction
float n[3] = {0}; // Surface normal
// Arbitrary starting support point
float v_len2 = FLT_MAX;
aabb_support(p, clip, v);
vec3_copy(x, ray_origin);
vec3_sub(v, x, p);
// Time of impact
float lambda = 0;
for (n_iter = 0;; ++n_iter) {
// Support point in C and new direction
float w[3];
aabb_support(p, clip, v);
vec3_sub(w, x, p);
float v_dot_w = vec3_dot(v, w);
if (v_dot_w > 0) {
float v_dot_r = vec3_dot(v, ray_direction);
if (v_dot_r >= 0) {
// We're past the origin, no collision
printf("GJK iterations: %d\n", n_iter);
return hit;
}
// Increment time to move along the ray
float prev_lambda = lambda;
lambda = lambda - v_dot_w / v_dot_r;
if (lambda >= t_max) {
// Going beyond the bounds of the ray being tested, no
// collision
printf("GJK iterations: %d\n", n_iter);
return hit;
}
if (prev_lambda == lambda) {
// We must have converged, assume collision
break;
}
// New point on the ray at time lambda
vec3_muls(x, ray_direction, lambda);
vec3_add(x, x, ray_origin);
v_len2 = FLT_MAX;
}
// P = P U {p}
assert(simplex_count < 4);
vec3_copy(P[simplex_count], p);
++simplex_count;
// Y = {x} - P
for (unsigned i = 0; i < simplex_count; ++i) {
vec3_sub(Y[i], x, P[i]);
}
char simplex[4] = {0};
puts("NEW SIMPLEX Y");
printf("(%f, %f, %f)\n", (float64)Y[0][0], (float64)Y[0][1], (float64)Y[0][2]);
printf("(%f, %f, %f)\n", (float64)Y[1][0], (float64)Y[1][1], (float64)Y[1][2]);
printf("(%f, %f, %f)\n", (float64)Y[2][0], (float64)Y[2][1], (float64)Y[2][2]);
printf("(%f, %f, %f)\n", (float64)Y[3][0], (float64)Y[3][1], (float64)Y[3][2]);
// Update v and P with the closest point in from the simplex Y to the
// origin.
switch (simplex_count) {
case 1:
vec3_copy(v, Y[0]);
simplex[0] = 1;
break;
case 2:
closest_point_line(v, simplex, Y);
break;
case 3:
closest_point_triangle(v, simplex, Y);
break;
case 4:
closest_point_tetrahedron(v, simplex, Y);
break;
default:
assert(0); // No pentatopes!
}
// Build new simplex with points from P
simplex_count = 0;
for (unsigned i = 0; i < 4; ++i) {
if (simplex[i]) {
vec3_copy(P[simplex_count], P[i]);
++simplex_count;
}
}
if (simplex_count == 4) {
// Simplex contains the origin, collision
break;
}
float prev_v_len2 = v_len2;
v_len2 = vec3_dot(v, v);
if (v_len2 <= FLT_EPSILON || prev_v_len2 < v_len2) {
// Converged, collision
printf("CONVERGED\n");
break;
}
// Update normal
vec3_copy(n, v);
}
printf("GJK iterations: %d\n", n_iter);
hit.hit = 1;
hit.t = lambda;
vec3_copy(hit.normal, n);
vec3_normalize(hit.normal);
printf("NORMAL %f, %f, %f\n", (float64)hit.normal[0], (float64)hit.normal[1], (float64)hit.normal[2]);
vec3_muls(x, ray_direction, hit.t);
vec3_add(x, x, ray_origin);
vec3_copy(hit.position, x);
//aabb_support(hit.position, C, hit.normal);
//
return hit;
}
struct ray_hit
gjk_swept_trace(float ray_origin[3], float ray_direction[3], struct aabb *A, struct aabb *B, float t_max)
{
unsigned k;
struct ray_hit hit = {0};
const float initial_tolerance = 0.0001f;
const float skin = 0.001f; // 0.5cm
const float convex_radius = 2.0f * skin;
// Simplecies being tested
unsigned simplex_count = 0;
float P[4][3];
float Q[4][3];
float Y[4][3];
float x[3] = {0};
float p[3], q[3]; // Current support points
float v[3], w[3]; // Current direction
float n[3]; // Current normal
// Arbitrary starting support point
float v_len2 = FLT_MAX;
vec3_zero(v);
vec3_zero(n);
aabb_support(p, A, v);
aabb_support(q, B, v);
vec3_inverse(q, q);
vec3_add(v, q, p);
// Time of impact
float lambda = 0;
float prev_v[3];
// Initial tolerance without taking skin radius into account
float tolerance = initial_tolerance; // Error^2
for (k = 0;; ++k) {
// Minkowski sum of B + (-A)
float inv_v[3];
vec3_inverse(inv_v, v);
aabb_support(q, B, v);
aabb_support(p, A, inv_v);
vec3_sub(w, q, p);
vec3_sub(w, x, w);
float v_dot_w = vec3_dot(v, w) - convex_radius * vec3_length(v);
if (v_dot_w > 0) {
float v_dot_r = vec3_dot(v, ray_direction);
if (v_dot_r >= 0) {
// We're past the origin, no collision
printf("GJK iterations: %d\n", k);
return hit;
}
// Increment time to move along the ray
float prev_lambda = lambda;
lambda = lambda - v_dot_w / v_dot_r;
if (lambda >= t_max) {
// Going beyond the bounds of the ray being tested, no
// collision
printf("GJK iterations: %d\n", k);
return hit;
}
if (prev_lambda == lambda) {
// We must have converged, assume collision
break;
}
// A, B not intersecting at t=0, add skin radius to tolerance
tolerance = initial_tolerance + (convex_radius * convex_radius);
// New point on the ray at time lambda
vec3_muls(x, ray_direction, lambda);
v_len2 = FLT_MAX;
}
// P = P U {p}, Q = Q U {q}
assert(simplex_count < 4);
vec3_copy(P[simplex_count], p);
vec3_copy(Q[simplex_count], q);
++simplex_count;
// Y = {x} - (Q - P)
for (unsigned i = 0; i < simplex_count; ++i) {
vec3_sub(Y[i], Q[i], P[i]);
vec3_sub(Y[i], x, Y[i]);
}
char simplex[4] = {0};
// Update v and P with the closest point in from the simplex Y to the
// origin.
switch (simplex_count) {
case 1:
vec3_copy(v, Y[0]);
simplex[0] = 1;
break;
case 2:
closest_point_line(v, simplex, Y);
break;
case 3:
closest_point_triangle(v, simplex, Y);
break;
case 4:
closest_point_tetrahedron(v, simplex, Y);
break;
default:
assert(0); // No pentatopes!
}
// Build new simplex with points from P and Q
simplex_count = 0;
for (unsigned i = 0; i < 4; ++i) {
if (simplex[i]) {
vec3_copy(P[simplex_count], P[i]);
vec3_copy(Q[simplex_count], Q[i]);
++simplex_count;
}
}
printf("LAMBDA: %f\n", lambda);
float prev_v_len2 = v_len2;
v_len2 = vec3_dot(v, v);
if (simplex_count == 4) {
// Simplex contains the origin, collision
printf("Simplex surrounds the origin, converged? %d\n", v_len2 <= tolerance);
break;
}
if (v_len2 <= FLT_EPSILON || prev_v_len2 < v_len2) {
// Converged, collision
printf("CONVERGED\n");
break;
}
if (simplex_count == 4) {
// Simplex contains the origin, collision
return hit;
}
if (v_len2 <= tolerance || v_len2 >= prev_v_len2) {
// Converged, collision
break;
}
// Update normal
vec3_copy(n, v);
}
// Correct for skin radius
float normalized_v[3] = {0};
if (vec3_dot(v, v) != 0) {
vec3_copy(normalized_v, v);
vec3_normalize(normalized_v);
}
float corrected_convex_radius[3];
vec3_muls(corrected_convex_radius, normalized_v, skin);
// Y = {x} - (Q - P) for contact generation
for (unsigned i = 0; i < simplex_count; ++i) {
vec3_sub(Y[i], Q[i], P[i]);
vec3_sub(Y[i], x, Y[i]);
}
float bc[3];
// Generate contact point on b
switch (simplex_count) {
case 1:
vec3_add(hit.position, Q[0], corrected_convex_radius);
break;
case 2: {
float bc_a[3], bc_b[3];
barycentric_coords_line(bc, Y);
// Contact point on B
vec3_muls(bc_a, Q[0], bc[0]);
vec3_muls(bc_b, Q[1], bc[1]);
vec3_add(hit.position, bc_a, bc_b);
vec3_add(hit.position, hit.position, corrected_convex_radius);
break;
}
case 3: // Triangle
case 4: { // Tetrahedron
// For the tetrahedron case we fallback to the last triangle simplex,
// as we cannot determine the contact point if the shapes are
// penetrating.
float bc_a[3], bc_b[3], bc_c[3];
barycentric_coords_triangle(bc, Y);
vec3_muls(bc_a, Q[0], bc[0]);
vec3_muls(bc_b, Q[1], bc[1]);
vec3_muls(bc_c, Q[2], bc[2]);
vec3_add(hit.position, bc_a, bc_b);
vec3_add(hit.position, hit.position, bc_c);
vec3_add(hit.position, hit.position, corrected_convex_radius);
break;
}
default:
printf("Invalid simplex count %d\n", simplex_count);
break;
}
printf("GJK iterations: %d\n", k);
printf("CONTACT POINT %f, %f, %f\n", (float64)hit.position[0], (float64)hit.position[1], (float64)hit.position[2]);
hit.hit = 1;
hit.t = lambda;
if (convex_radius > 0) {
vec3_copy(hit.normal, v);
} else {
// Use normal from previous iteration, without a skin the last
// iteration produces a normal that is too small and will cause
// problems.
vec3_copy(hit.normal, n);
}
vec3_normalize(hit.normal);
printf("NORMAL %f, %f, %f\n", (float64)hit.normal[0], (float64)hit.normal[1], (float64)hit.normal[2]);
return hit;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment