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

Pure JavaScript game engine. Fully featured and production ready.

import { assert } from "../../../core/assert.js";
import { array_copy } from "../../../core/collection/array/array_copy.js";
import { array_swap } from "../../../core/collection/array/array_swap.js";
import { v3_compute_triangle_normal } from "../../../core/geom/3d/triangle/v3_compute_triangle_normal.js";
import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
import { v3_dot_array_array } from "../../../core/geom/vec3/v3_dot_array_array.js";

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

const EPA_TOLERANCE = 0.0001;
const EPA_MAX_NUM_FACES = 64;
const EPA_MAX_NUM_LOOSE_EDGES = 32;
const EPA_MAX_NUM_ITERATIONS = 64;

const FACE_ELEMENT_COUNT = 3 * 4;

const EDGE_ELEMENT_COUNT = 2 * 3;

//Array of faces, each with 3 verts and a normal
const faces = new Float32Array(EPA_MAX_NUM_FACES * FACE_ELEMENT_COUNT);

/**
 * keep track of edges we need to fix after removing faces
 * @type {Float32Array}
 */
const loose_edges = new Float32Array(EPA_MAX_NUM_LOOSE_EDGES * EDGE_ELEMENT_COUNT);

const scratch_v3 = new Float32Array(3);

/**
 *
 * @param {number[]} target
 * @param {number} target_index
 * @param {number[]} v
 */
function write_v3(target, target_index, v) {
    array_copy(v, 0, faces, target_index * 3, 3);
}

/**
 *
 * @param {number[]|Float32Array} faces
 * @param {number} index
 * @param {number[]} a
 * @param {number[]} b
 * @param {number[]} c
 */
function write_face(faces, index, a, b, c) {
    const index4 = index * 4;

    write_v3(faces, index4 + 0, a);
    write_v3(faces, index4 + 1, b);
    write_v3(faces, index4 + 2, c);

    const normal_offset = (index4 + 3) * 3;

    v3_compute_triangle_normal(
        faces, normal_offset,
        a[0], a[1], a[2],
        b[0], b[1], b[2],
        c[0], c[1], c[2]
    );
}

/**
 * TODO: needs to be tested thoroughly, intended to be working with GJK
 * @param {number[]} result
 * @param {number} result_offset
 * @param {number[]} a
 * @param {number[]} b
 * @param {number[]} c
 * @param {number[]} d
 * @param {AbstractShape3D} coll1
 * @param {AbstractShape3D} coll2
 */
export function expanding_polytope_algorithm(
    result, result_offset,
    a, b, c, d,
    coll1, coll2
) {
    /*
     Adapted from https://github.com/kevinmoran/GJK/blob/master/GJK.h

     "GJK + Expanding Polytope Algorithm - Implementation and Visualization"  https://www.youtube.com/watch?v=6rgiPrzqt9w
     */

    //Init with final simplex from GJK
    write_face(faces, 0, a, b, c);
    write_face(faces, 1, a, c, d);
    write_face(faces, 2, a, d, b);
    write_face(faces, 3, b, d, c);

    let num_faces = 4;
    let closest_face;

    for (let iterations = 0; iterations < EPA_MAX_NUM_ITERATIONS; iterations++) {

        //Find face that's closest to origin

        let min_dist = v3_dot_array_array(faces, 0, faces, 3 * 3);
        closest_face = 0;

        for (let i = 1; i < num_faces; i++) {
            const dist = v3_dot_array_array(faces, i * FACE_ELEMENT_COUNT, faces, i * FACE_ELEMENT_COUNT + 3 * 3);

            if (dist < min_dist) {
                min_dist = dist;
                closest_face = i;
            }

        }

        //search normal to face that's closest to origin
        const closest_face_normal_offset = closest_face * FACE_ELEMENT_COUNT + 3 * 3;

        const search_dir_x = faces[closest_face_normal_offset];
        const search_dir_y = faces[closest_face_normal_offset + 1];
        const search_dir_z = faces[closest_face_normal_offset + 2];

        if (search_dir_x === 0 && search_dir_y === 0 && search_dir_z === 0) {
            debugger;
        }

        // build new support point to expand polytope

        coll2.support(scratch_v3, 0, search_dir_x, search_dir_y, search_dir_z);

        const support2_x = scratch_v3[0];
        const support2_y = scratch_v3[1];
        const support2_z = scratch_v3[2];

        coll1.support(scratch_v3, 0, -search_dir_x, -search_dir_y, -search_dir_z);

        const support1_x = scratch_v3[0];
        const support1_y = scratch_v3[1];
        const support1_z = scratch_v3[2];

        const p_x = support2_x - support1_x;
        const p_y = support2_y - support1_y;
        const p_z = support2_z - support1_z;

        const dot_p_search_dir = v3_dot(p_x, p_y, p_z, search_dir_x, search_dir_y, search_dir_z);

        if (dot_p_search_dir - min_dist < EPA_TOLERANCE) {
            //Convergence (new point is not significantly further from origin)

            result[result_offset] = search_dir_x * dot_p_search_dir;
            result[result_offset + 1] = search_dir_y * dot_p_search_dir;
            result[result_offset + 2] = search_dir_z * dot_p_search_dir;

            return;
        }

        let num_loose_edges = 0;

        //Find all triangles that are facing p
        for (let i = 0; i < num_faces; i++) {
            const face_offset = i * FACE_ELEMENT_COUNT;

            const face_normal_address = face_offset + 3 * 3;
            const face_a_address = face_offset;

            const face_normal_x = faces[face_normal_address];
            const face_normal_y = faces[face_normal_address + 1];
            const face_normal_z = faces[face_normal_address + 2];

            const face_a_x = faces[face_a_address];
            const face_a_y = faces[face_a_address + 1];
            const face_a_z = faces[face_a_address + 2];

            const pa_x = p_x - face_a_x;
            const pa_y = p_y - face_a_y;
            const pa_z = p_z - face_a_z;

            if (v3_dot(face_normal_x, face_normal_y, face_normal_z, pa_x, pa_y, pa_z) <= 0) {
                continue;
            }

            // triangle i faces p, remove it
            for (let j = 0; j < 3; j++) //Three edges per face
            {
                const a_offset = j * 3;

                const current_edge_ax = faces[face_offset + a_offset];
                const current_edge_ay = faces[face_offset + a_offset + 1];
                const current_edge_az = faces[face_offset + a_offset + 2];

                const b_offset = ((j + 1) % 3) * 3;

                const current_edge_bx = faces[face_offset + b_offset];
                const current_edge_by = faces[face_offset + b_offset + 1];
                const current_edge_bz = faces[face_offset + b_offset + 2];

                // vec3 current_edge[2] = { faces[i][j], faces[i][(j + 1) % 3] };

                let found_edge = false;

                //Check if current edge is already in list
                for (let k = 0; k < num_loose_edges; k++) {
                    const loose_edge_offset = k * EDGE_ELEMENT_COUNT;

                    const loose_edge_ax = loose_edges[loose_edge_offset];
                    const loose_edge_ay = loose_edges[loose_edge_offset + 1];
                    const loose_edge_az = loose_edges[loose_edge_offset + 2];

                    const loose_edge_bx = loose_edges[loose_edge_offset + 3];
                    const loose_edge_by = loose_edges[loose_edge_offset + 4];
                    const loose_edge_bz = loose_edges[loose_edge_offset + 5];


                    // if (loose_edges[k][1] === current_edge[0] && loose_edges[k][0] === current_edge[1]) {
                    if (
                        (
                            loose_edge_ax === current_edge_bx
                            && loose_edge_ay === current_edge_by
                            && loose_edge_az === current_edge_bz
                        )
                        && (
                            loose_edge_bx === current_edge_ax
                            && loose_edge_by === current_edge_ay
                            && loose_edge_bz === current_edge_az
                        )
                    ) {
                        //Edge is already in the list, remove it
                        //THIS ASSUMES EDGE CAN ONLY BE SHARED BY 2 TRIANGLES (which should be true)
                        //THIS ALSO ASSUMES SHARED EDGE WILL BE REVERSED IN THE TRIANGLES (which
                        //should be true provided every triangle is wound CCW)

                        // Overwrite current edge with last edge in list
                        loose_edges.copyWithin(loose_edge_offset, (num_loose_edges - 1) * EDGE_ELEMENT_COUNT, (num_loose_edges) * EDGE_ELEMENT_COUNT);

                        num_loose_edges--;
                        found_edge = true;

                        //exit loop because edge can only be shared once
                        break;
                    }
                }

                if (!found_edge) { //add current edge to list
                    // assert(num_loose_edges<EPA_MAX_NUM_LOOSE_EDGES);

                    assert.lessThan(num_loose_edges, EPA_MAX_NUM_LOOSE_EDGES);

                    if (num_loose_edges >= EPA_MAX_NUM_LOOSE_EDGES) {
                        // this should not happen, but if it does it is best to be "kind of right" than to fail completely
                        break;
                    }

                    const last_edge_offset = num_loose_edges * EDGE_ELEMENT_COUNT;

                    loose_edges[last_edge_offset] = current_edge_ax;
                    loose_edges[last_edge_offset + 1] = current_edge_ay;
                    loose_edges[last_edge_offset + 2] = current_edge_az;

                    loose_edges[last_edge_offset + 3] = current_edge_bx;
                    loose_edges[last_edge_offset + 4] = current_edge_by;
                    loose_edges[last_edge_offset + 5] = current_edge_bz;

                    num_loose_edges++;
                }
            }

            // move last face to fill the removed element
            faces.copyWithin(i * FACE_ELEMENT_COUNT, (num_faces - 1) * FACE_ELEMENT_COUNT, num_faces * FACE_ELEMENT_COUNT);

            num_faces--;
            i--;

        }

        //Reconstruct polytope with p added
        for (let i = 0; i < num_loose_edges; i++) {
            // assert(num_faces<EPA_MAX_NUM_FACES);

            assert.lessThan(num_faces, EPA_MAX_NUM_FACES);

            if (num_faces >= EPA_MAX_NUM_FACES) {
                // should never happen
                break;
            }

            const face_offset = num_faces * FACE_ELEMENT_COUNT;

            const loose_edge_offset = i * EDGE_ELEMENT_COUNT;

            array_copy(
                loose_edges, loose_edge_offset,
                faces, face_offset,
                EDGE_ELEMENT_COUNT
            );

            faces[face_offset + 3 * 2] = p_x;
            faces[face_offset + 3 * 2 + 1] = p_y;
            faces[face_offset + 3 * 2 + 2] = p_z;

            // construct normal
            const face_normal_offset = face_offset + 3 * 3;
            construct_normal(face_normal_offset, i, p_x, p_y, p_z);

            //Check for wrong normal to maintain CCW winding
            const bias = 0.000001; //in case dot result is only slightly < 0 (because origin is on face)

            const dot = v3_dot_array_array(faces, face_offset, faces, face_normal_offset)

            if (dot + bias < 0) {
                array_swap(faces, face_offset, faces, face_offset + 3, 3);
                // invert normal
                v3_negate_array(faces, face_normal_offset, faces, face_normal_offset);
            }

            num_faces++;
        }
    }

    console.warn("EPA did not converge");

    //Return most recent closest point
    const closest_face_offset = closest_face * FACE_ELEMENT_COUNT;
    const closest_face_normal_offset = closest_face_offset + 3 * 3;

    const dot = v3_dot_array_array(faces, closest_face_offset, faces, closest_face_normal_offset);

    result[result_offset] = faces[closest_face_normal_offset + 0] * dot;
    result[result_offset + 1] = faces[closest_face_normal_offset + 1] * dot;
    result[result_offset + 2] = faces[closest_face_normal_offset + 2] * dot;

}

/**
 *
 * @param {number} result_offset
 * @param {number} edge_index
 * @param {number} p_x
 * @param {number} p_y
 * @param {number} p_z
 */
function construct_normal(result_offset, edge_index, p_x, p_y, p_z) {
    const edge_offset = edge_index * EDGE_ELEMENT_COUNT;

    const edge_ax = loose_edges[edge_offset];
    const edge_ay = loose_edges[edge_offset + 1];
    const edge_az = loose_edges[edge_offset + 2];

    const edge_bx = loose_edges[edge_offset + 3];
    const edge_by = loose_edges[edge_offset + 4];
    const edge_bz = loose_edges[edge_offset + 5];

    v3_compute_triangle_normal(
        faces, result_offset,
        edge_ax, edge_ay, edge_az,
        edge_bx, edge_by, edge_bz,
        p_x, p_y, p_z
    );
}