@woosh/meep-engine/src/engine/physics/gjk/gjk.js

Pure JavaScript game engine. Fully featured and production ready.

import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
import { v3_length } from "../../../core/geom/vec3/v3_length.js";

const GJK_MAX_ITERATIONS = 64;

/**
 * Module-level scratch buffers to avoid per-call allocations.
 */
const scratch_support_a = new Float32Array(3);
const scratch_support_b = new Float32Array(3);
const scratch_dir = new Float32Array(3);

/**
 * Compute the Minkowski difference support point for two shapes.
 * support(A-B, d) = support(A, d) - support(B, -d)
 *
 * The direction is normalized before being passed to the shape support
 * functions, since {@link AbstractShape3D#support} assumes a unit vector.
 *
 * @param {number[]|Float32Array} result
 * @param {number} result_offset
 * @param {AbstractShape3D} shape_a
 * @param {AbstractShape3D} shape_b
 * @param {number} dir_x
 * @param {number} dir_y
 * @param {number} dir_z
 */
function minkowski_support(
    result, result_offset,
    shape_a,
    shape_b,
    dir_x, dir_y, dir_z
) {
    // Normalize direction — support() contract requires a unit vector
    const len = v3_length(dir_x, dir_y, dir_z);

    if (len > 0) {
        const inv = 1 / len;
        dir_x *= inv;
        dir_y *= inv;
        dir_z *= inv;
    }

    shape_a.support(scratch_support_a, 0, dir_x, dir_y, dir_z);
    shape_b.support(scratch_support_b, 0, -dir_x, -dir_y, -dir_z);

    result[result_offset] = scratch_support_a[0] - scratch_support_b[0];
    result[result_offset + 1] = scratch_support_a[1] - scratch_support_b[1];
    result[result_offset + 2] = scratch_support_a[2] - scratch_support_b[2];
}

/**
 * GJK intersection test for two convex shapes.
 *
 * Determines whether two convex shapes overlap by iteratively building a simplex
 * in the Minkowski difference space. If the origin is enclosed by the simplex,
 * the shapes intersect.
 *
 * Adapted from https://github.com/kevinmoran/GJK/blob/master/GJK.h
 *
 * @param {number[]|Float32Array} simplex Working buffer for simplex vertices (4 vec3s). Must have length >= 12.
 * @param {AbstractShape3D} shape_a
 * @param {AbstractShape3D} shape_b
 * @returns {boolean} true if the shapes intersect
 */
export function gjk(simplex, shape_a, shape_b) {

    const dir = scratch_dir;

    // Initial search direction — arbitrary, (1,0,0)

    // Get initial support point (simplex point C)
    minkowski_support(
        simplex, 6,
        shape_a, shape_b,
        1, 0, 0
    );

    // Search toward the origin from C
    dir[0] = -simplex[6];
    dir[1] = -simplex[7];
    dir[2] = -simplex[8];

    if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
        // Origin is exactly on the first support point — intersection
        return true;
    }

    // Get second support point (simplex point B)
    minkowski_support(simplex, 3, shape_a, shape_b, dir[0], dir[1], dir[2]);

    // B didn't pass the origin — no intersection possible
    if (v3_dot(simplex[3], simplex[4], simplex[5], dir[0], dir[1], dir[2]) < 0) {
        return false;
    }

    // Line case: compute direction perpendicular to line segment BC toward origin
    // CB = C - B
    const cb_x = simplex[6] - simplex[3];
    const cb_y = simplex[7] - simplex[4];
    const cb_z = simplex[8] - simplex[5];

    // BO = -B (origin - B)
    const bo_x = -simplex[3];
    const bo_y = -simplex[4];
    const bo_z = -simplex[5];

    // dir = CB × BO × CB (triple cross product)
    triple_cross_product(dir, cb_x, cb_y, cb_z, bo_x, bo_y, bo_z);

    // Handle degenerate case where origin is on the line segment
    if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
        // Origin is on the line segment — pick any perpendicular direction
        // Use the cross product of CB with an arbitrary axis
        // CB × (1,0,0)
        dir[0] = cb_y;
        dir[1] = -cb_x;
        dir[2] = 0;

        if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
            // CB is parallel to (1,0,0), use (0,1,0)
            dir[0] = -cb_z;
            dir[1] = 0;
            dir[2] = cb_x;
        }
    }

    let simplex_size = 2; // we have B and C

    for (let iterations = 0; iterations < GJK_MAX_ITERATIONS; iterations++) {
        // Get new support point A
        minkowski_support(simplex, 0, shape_a, shape_b, dir[0], dir[1], dir[2]);

        const a_x = simplex[0];
        const a_y = simplex[1];
        const a_z = simplex[2];

        // Check if A passed the origin
        if (v3_dot(a_x, a_y, a_z, dir[0], dir[1], dir[2]) < 0) {
            return false; // no intersection
        }

        simplex_size++;

        if (simplex_size === 3) {
            // Triangle case
            update_simplex_triangle(dir, simplex);
        } else {
            // Tetrahedron case
            if (update_simplex_tetrahedron(dir, simplex)) {
                // Origin is inside the tetrahedron — intersection!
                return true;
            }

            // The simplex was reduced to a triangle, so keep size at 3
            simplex_size = 3;
        }
    }

    // Did not converge — assume no intersection
    return false;
}

/**
 * Compute the triple cross product: (a × b) × a
 *
 * @param {number[]|Float32Array} result output vec3
 * @param {number} ax
 * @param {number} ay
 * @param {number} az
 * @param {number} bx
 * @param {number} by
 * @param {number} bz
 */
function triple_cross_product(result, ax, ay, az, bx, by, bz) {
    // First: t = a × b
    const tx = ay * bz - az * by;
    const ty = az * bx - ax * bz;
    const tz = ax * by - ay * bx;

    // Then: t × a
    result[0] = ty * az - tz * ay;
    result[1] = tz * ax - tx * az;
    result[2] = tx * ay - ty * ax;
}

/**
 * Update simplex for the triangle case (3 points: A, B, C).
 *
 * Determines the Voronoi region of the origin relative to triangle ABC
 * and updates the search direction accordingly.
 *
 * simplex layout: [A(0-2), B(3-5), C(6-8)]
 *
 * @param {number[]|Float32Array} search_dir output: next search direction
 * @param {Float32Array} simplex
 */
function update_simplex_triangle(search_dir, simplex) {
    const a_x = simplex[0], a_y = simplex[1], a_z = simplex[2];
    const b_x = simplex[3], b_y = simplex[4], b_z = simplex[5];
    const c_x = simplex[6], c_y = simplex[7], c_z = simplex[8];

    // AB = B - A
    const ab_x = b_x - a_x;
    const ab_y = b_y - a_y;
    const ab_z = b_z - a_z;

    // AC = C - A
    const ac_x = c_x - a_x;
    const ac_y = c_y - a_y;
    const ac_z = c_z - a_z;

    // AO = -A (origin - A)
    const ao_x = -a_x;
    const ao_y = -a_y;
    const ao_z = -a_z;

    // Normal of triangle ABC
    const abc_x = ab_y * ac_z - ab_z * ac_y;
    const abc_y = ab_z * ac_x - ab_x * ac_z;
    const abc_z = ab_x * ac_y - ab_y * ac_x;

    // Test edge AC
    // ABC × AC
    const abc_cross_ac_x = abc_y * ac_z - abc_z * ac_y;
    const abc_cross_ac_y = abc_z * ac_x - abc_x * ac_z;
    const abc_cross_ac_z = abc_x * ac_y - abc_y * ac_x;

    if (v3_dot(abc_cross_ac_x, abc_cross_ac_y, abc_cross_ac_z, ao_x, ao_y, ao_z) > 0) {
        // Origin is on the AC side
        // Reduce simplex to line AC (A stays as A, C moves to B position)
        simplex[3] = c_x;
        simplex[4] = c_y;
        simplex[5] = c_z;

        // New direction: AC × AO × AC
        triple_cross_product(search_dir, ac_x, ac_y, ac_z, ao_x, ao_y, ao_z);
        return;
    }

    // Test edge AB
    // AB × ABC
    const ab_cross_abc_x = ab_y * abc_z - ab_z * abc_y;
    const ab_cross_abc_y = ab_z * abc_x - ab_x * abc_z;
    const ab_cross_abc_z = ab_x * abc_y - ab_y * abc_x;

    if (v3_dot(ab_cross_abc_x, ab_cross_abc_y, ab_cross_abc_z, ao_x, ao_y, ao_z) > 0) {
        // Origin is on the AB side
        // Keep simplex as line AB (C slot is cleared by not using it)
        simplex[6] = simplex[3];
        simplex[7] = simplex[4];
        simplex[8] = simplex[5];

        // New direction: AB × AO × AB
        triple_cross_product(search_dir, ab_x, ab_y, ab_z, ao_x, ao_y, ao_z);
        return;
    }

    // Origin is above or below the triangle
    if (v3_dot(abc_x, abc_y, abc_z, ao_x, ao_y, ao_z) > 0) {
        // Origin is above — search in normal direction
        // Simplex stays as A, B, C
        search_dir[0] = abc_x;
        search_dir[1] = abc_y;
        search_dir[2] = abc_z;
    } else {
        // Origin is below — swap B and C, search in -normal
        const tmp_x = simplex[3], tmp_y = simplex[4], tmp_z = simplex[5];
        simplex[3] = simplex[6];
        simplex[4] = simplex[7];
        simplex[5] = simplex[8];
        simplex[6] = tmp_x;
        simplex[7] = tmp_y;
        simplex[8] = tmp_z;

        search_dir[0] = -abc_x;
        search_dir[1] = -abc_y;
        search_dir[2] = -abc_z;
    }
}

/**
 * Update simplex for the tetrahedron case (4 points: A, B, C, D).
 *
 * Tests which face of the tetrahedron is closest to the origin and
 * reduces the simplex accordingly. Returns true if the origin is
 * inside the tetrahedron.
 *
 * simplex layout: [A(0-2), B(3-5), C(6-8), D(9-11)]
 *
 * @param {number[]|Float32Array} search_dir output: next search direction (only written when returning false)
 * @param {Float32Array} simplex
 * @returns {boolean} true if origin is inside the tetrahedron
 */
function update_simplex_tetrahedron(search_dir, simplex) {
    const a_x = simplex[0], a_y = simplex[1], a_z = simplex[2];
    const b_x = simplex[3], b_y = simplex[4], b_z = simplex[5];
    const c_x = simplex[6], c_y = simplex[7], c_z = simplex[8];
    const d_x = simplex[9], d_y = simplex[10], d_z = simplex[11];

    // AB, AC, AD vectors
    const ab_x = b_x - a_x, ab_y = b_y - a_y, ab_z = b_z - a_z;
    const ac_x = c_x - a_x, ac_y = c_y - a_y, ac_z = c_z - a_z;
    const ad_x = d_x - a_x, ad_y = d_y - a_y, ad_z = d_z - a_z;

    // AO = origin - A = -A
    const ao_x = -a_x, ao_y = -a_y, ao_z = -a_z;

    // Face normals (outward-facing from A's perspective)
    // ABC normal
    const abc_x = ab_y * ac_z - ab_z * ac_y;
    const abc_y = ab_z * ac_x - ab_x * ac_z;
    const abc_z = ab_x * ac_y - ab_y * ac_x;

    // ACD normal
    const acd_x = ac_y * ad_z - ac_z * ad_y;
    const acd_y = ac_z * ad_x - ac_x * ad_z;
    const acd_z = ac_x * ad_y - ac_y * ad_x;

    // ADB normal
    const adb_x = ad_y * ab_z - ad_z * ab_y;
    const adb_y = ad_z * ab_x - ad_x * ab_z;
    const adb_z = ad_x * ab_y - ad_y * ab_x;

    // Test each face to see if the origin is on the outside

    // Check face ABC (opposite to D)
    const abc_test = v3_dot(abc_x, abc_y, abc_z, ao_x, ao_y, ao_z);
    // Make sure normal points away from D
    const abc_d_side = v3_dot(abc_x, abc_y, abc_z, ad_x, ad_y, ad_z);

    if (abc_test * abc_d_side < 0) {
        // Origin is on the opposite side of ABC from D
        // Reduce to triangle ABC: D is dropped
        // simplex = [A, B, C] — A(0), B(3), C(6) already in place
        update_simplex_triangle(search_dir, simplex);
        return false;
    }

    // Check face ACD (opposite to B)
    const acd_test = v3_dot(acd_x, acd_y, acd_z, ao_x, ao_y, ao_z);
    const acd_b_side = v3_dot(acd_x, acd_y, acd_z, ab_x, ab_y, ab_z);

    if (acd_test * acd_b_side < 0) {
        // Origin is outside face ACD
        // Reduce to triangle ACD: replace B with D
        simplex[3] = c_x;
        simplex[4] = c_y;
        simplex[5] = c_z;
        simplex[6] = d_x;
        simplex[7] = d_y;
        simplex[8] = d_z;

        update_simplex_triangle(search_dir, simplex);
        return false;
    }

    // Check face ADB (opposite to C)
    const adb_test = v3_dot(adb_x, adb_y, adb_z, ao_x, ao_y, ao_z);
    const adb_c_side = v3_dot(adb_x, adb_y, adb_z, ac_x, ac_y, ac_z);

    if (adb_test * adb_c_side < 0) {
        // Origin is outside face ADB
        // Reduce to triangle ADB: replace C with B, B with D
        simplex[6] = b_x;
        simplex[7] = b_y;
        simplex[8] = b_z;
        simplex[3] = d_x;
        simplex[4] = d_y;
        simplex[5] = d_z;

        update_simplex_triangle(search_dir, simplex);
        return false;
    }

    // Origin is inside the tetrahedron
    return true;
}