@perplexdotgg/bounce 1.0.0

package/src/collision/epa/EpaConvexHullBuilder.ts

@@ -0,0 +1,397 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+import { TriangleFactory } from "./TriangleFactory";
+import { Edges, NewTriangles, Points, TriangleQueue } from "./arrays";
+import { Triangle } from "./structs";
+
+interface StackEntry {
+  triangle: Triangle | null;
+  edge: number;
+  iter: number;
+}
+
+let iteration = 0;
+
+export interface EpaConvexHullBuilderSettings {
+  // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2
+  // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2
+  // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2.
+
+  /**
+   * Maximum number of triangles in the convex hull.
+   */
+  maxTriangles: number;
+
+  /**
+   * Maximum number of points in the convex hull.
+   */
+  maxPoints: number;
+
+  /**
+   * Maximum edge length to consider when finding edges.
+   */
+  maxEdgeLength: number;
+
+  /**
+   * Minimum area of a triangle before it is added to the priority queue.
+   */
+  minTriangleArea: number;
+
+  /**
+   * Epsilon value used to determine if a point is in the interior of a triangle.
+   */
+  barycentricEpsilon: number;
+}
+
+// Link triangle edge to other triangle edge
+function linkTriangle(inT1: Triangle, inEdge1: number, inT2: Triangle, inEdge2: number) {
+  // #v-ifdef DEV
+  assert(inEdge1 >= 0 && inEdge1 < 3, "Edge index 1 out of bounds");
+  assert(inEdge2 >= 0 && inEdge2 < 3, "Edge index 2 out of bounds");
+  // #v-endif
+
+  const e1 = inT1.edge[inEdge1];
+  const e2 = inT2.edge[inEdge2];
+
+  // #v-ifdef DEV
+  // Check not connected yet
+  assert(e1.neighbourTriangle === null, "Edge 1 already has a neighbour triangle");
+  assert(e2.neighbourTriangle === null, "Edge 2 already has a neighbour triangle");
+  // #v-endif
+
+  // #v-ifdef DEV
+  // Check vertices match
+  assert(
+    e1.startIndex === inT2.getNextEdge(inEdge2).startIndex,
+    "Edge 1 start index does not match Edge 2 next edge start index"
+  );
+  assert(
+    e2.startIndex === inT1.getNextEdge(inEdge1).startIndex,
+    "Edge 2 start index does not match Edge 1 next edge start index"
+  );
+  // #v-endif
+
+  // Link up
+  e1.neighbourTriangle = inT2;
+  e1.neighbourEdge = inEdge2;
+  e2.neighbourTriangle = inT1;
+  e2.neighbourEdge = inEdge1;
+}
+
+export class EpaConvexHullBuilder {
+  settings: EpaConvexHullBuilderSettings;
+  triangleFactory: TriangleFactory;
+  triangleQueue: TriangleQueue;
+  positions: Points | null;
+  stack: Array<StackEntry>;
+  edges: Edges;
+
+  static createDefaultSettings(): EpaConvexHullBuilderSettings {
+    return {
+      maxTriangles: 256,
+      maxPoints: 256 / 2,
+      maxEdgeLength: 128,
+      minTriangleArea: 1e-10,
+      barycentricEpsilon: 1e-3,
+    };
+  }
+
+  constructor(settings: Partial<EpaConvexHullBuilderSettings> = {}) {
+    this.settings = {
+      ...EpaConvexHullBuilder.createDefaultSettings(),
+      ...settings,
+    };
+    this.triangleFactory = new TriangleFactory(
+      this.settings.maxTriangles,
+      this.settings.minTriangleArea,
+      this.settings.barycentricEpsilon
+    );
+    this.triangleQueue = new TriangleQueue(this.settings.maxTriangles);
+    this.positions = null;
+
+    const stack = new Array<StackEntry>(this.settings.maxEdgeLength);
+    for (let i = 0; i < this.settings.maxEdgeLength; ++i) {
+      stack[i] = { triangle: null, edge: 0, iter: -1 };
+    }
+    this.stack = stack;
+
+    this.edges = new Edges(this.settings.maxEdgeLength);
+  }
+
+  // Create a new triangle
+  createTriangle(inIdx1: number, inIdx2: number, inIdx3: number) {
+    // Call provider to create triangle
+
+    const t = this.triangleFactory.createTriangle(inIdx1, inIdx2, inIdx3, this.positions!.data());
+    if (!t) {
+      return null;
+    }
+
+    return t;
+  }
+
+  // Initialize the hull with 3 points
+  initialize(inIdx1: number, inIdx2: number, inIdx3: number) {
+    // console.log(iteration);
+    // if (iteration === 289) {
+    //   debugger;
+    // }
+    // iteration++;
+    // Release triangles
+    this.triangleFactory.clear();
+    this.triangleQueue.clear();
+
+    // Create triangles (back to back)
+    const t1 = this.createTriangle(inIdx1, inIdx2, inIdx3);
+    const t2 = this.createTriangle(inIdx1, inIdx3, inIdx2);
+
+    // #v-ifdef DEV
+    // assert
+    if (!t1 || !t2) {
+      throw new Error("Failed to create triangles");
+    }
+    // #v-endif
+
+    // Link triangles edges
+    linkTriangle(t1, 0, t2, 2);
+    linkTriangle(t1, 1, t2, 1);
+    linkTriangle(t1, 2, t2, 0);
+    // debugger;
+
+    // Always add both triangles to the priority queue
+    this.triangleQueue.pushBack(t1);
+    this.triangleQueue.pushBack(t2);
+  }
+
+  // Check if there's another triangle to process from the queue
+  hasNextTriangle() {
+    return !this.triangleQueue.empty();
+  }
+
+  // Access to the next closest triangle to the origin (won't remove it from the queue).
+  peekClosestTriangleInQueue() {
+    return this.triangleQueue.peekClosest();
+  }
+
+  // Access to the next closest triangle to the origin and remove it from the queue.
+  popClosestTriangleFromQueue() {
+    return this.triangleQueue.popClosest();
+  }
+
+  // Find the triangle on which inPosition is the furthest to the front
+  // Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX).
+  findFacingTriangle(inPosition: Vec3, outBestDistSq: { value: number }) {
+    let best: Triangle | null = null;
+    let bestDistSq = 0.0;
+
+    for (const t of this.triangleQueue) {
+      // #v-ifdef DEV
+      // assert
+      if (!t) {
+        throw new Error("Triangle is null in triangle queue");
+      }
+      // #v-endif
+
+      if (!t.removed) {
+        vectorAB.subtractVectors(inPosition, t.centroid);
+        const dot = t.normal.dot(vectorAB);
+        if (dot > 0.0) {
+          const distSq = (dot * dot) / t.normal.squaredLength();
+          if (distSq > bestDistSq) {
+            best = t;
+            bestDistSq = distSq;
+          }
+        }
+      }
+    }
+
+    outBestDistSq.value = bestDistSq;
+    return best;
+  }
+
+  // Free a triangle
+  freeTriangle(inT: Triangle) {
+    // #v-ifdef DEV
+    assert(inT.removed, "Triangle is not removed, cannot free it");
+    for (const edge of inT.edge) {
+      assert(edge.neighbourTriangle === null, "Edge still has a neighbour triangle when freeing triangle");
+    }
+    // #v-endif
+
+    this.triangleFactory.freeTriangle(inT);
+  }
+
+  // Unlink this triangle
+  unlinkTriangle(inT: Triangle) {
+    // Unlink from neighbours
+    for (let i = 0; i < 3; ++i) {
+      const edge = inT.edge[i];
+      if (edge.neighbourTriangle !== null) {
+        const neighbourEdge = edge.neighbourTriangle.edge[edge.neighbourEdge];
+
+        // #v-ifdef DEV
+        // Validate that neighbour points to us
+        assert(neighbourEdge.neighbourTriangle === inT, "Neighbour edge does not point to this triangle");
+        assert(neighbourEdge.neighbourEdge === i, "Neighbour edge does not point to this triangle edge");
+        // #v-endif
+
+        // Unlink
+        neighbourEdge.neighbourTriangle = null;
+        edge.neighbourTriangle = null;
+      }
+    }
+
+    // If this triangle is not in the priority queue, we can delete it now
+    if (!inT.inQueue) {
+      this.freeTriangle(inT);
+    }
+  }
+
+  // Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex.
+  // Will flag all those triangles for removal.
+  findEdge(inFacingTriangle: Triangle, inVertex: Vec3, outEdges: Edges) {
+    // #v-ifdef DEV
+    // Assert that we were given an empty array
+    assert(outEdges.empty(), "Output edges array is not empty");
+    // #v-endif
+
+    // #v-ifdef DEV
+    // Should start with a facing triangle
+    assert(inFacingTriangle.isFacing(inVertex), "Facing triangle does not face the vertex");
+    // #v-endif
+
+    // Flag as removed
+    inFacingTriangle.removed = true;
+
+    // Instead of recursing, we build our own stack with the information we need
+    const stack: StackEntry[] = this.stack;
+    let curStackPos = 0;
+
+    // Start with the triangle / edge provided
+    stack[0].triangle = inFacingTriangle;
+    stack[0].edge = 0;
+    stack[0].iter = -1; // Start with edge 0 (is incremented below before use)
+
+    // Next index that we expect to find, if we don't then there are 'islands'
+    let nextExpectedStartIdx = -1;
+
+    while (true) {
+      // Get current stack entry
+      const curEntry = stack[curStackPos];
+
+      // Next iteration
+      if (++curEntry.iter >= 3) {
+        // This triangle needs to be removed, unlink it now
+        this.unlinkTriangle(curEntry.triangle!);
+
+        // Pop from stack
+        if (--curStackPos < 0) {
+          break;
+        }
+      } else {
+        // Visit neighbour
+        const e = curEntry.triangle!.edge[(curEntry.edge + curEntry.iter) % 3];
+        const n = e.neighbourTriangle;
+        if (n !== null && !n.removed) {
+          // Check if vertex is on the front side of this triangle
+          if (n.isFacing(inVertex)) {
+            // Vertex on front, this triangle needs to be removed
+            n.removed = true;
+
+            // Add element to the stack of elements to visit
+            curStackPos++;
+            // #v-ifdef DEV
+            assert(curStackPos < this.settings.maxEdgeLength, "Stack overflow in findEdge");
+            // #v-endif
+            const newEntry = stack[curStackPos];
+            newEntry.triangle = n;
+            newEntry.edge = e.neighbourEdge;
+            newEntry.iter = 0; // Is incremented before use, we don't need to test this edge again since we came from it
+          } else {
+            // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means
+            // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar
+            // triangles as before and some behind the point. At this point we just abort adding the point because
+            // we've reached numerical precision.
+            // Note that we do not need to test if the first and last edge connect, since when there are islands
+            // there should be at least 2 disconnects.
+            if (e.startIndex !== nextExpectedStartIdx && nextExpectedStartIdx !== -1) {
+              return false;
+            }
+
+            // Next expected index is the start index of our neighbour's edge
+            nextExpectedStartIdx = n.edge[e.neighbourEdge].startIndex;
+
+            // Vertex behind, keep edge
+            outEdges.pushBackCopy(e);
+          }
+        }
+      }
+    }
+
+    // debugger;
+    // console.log(outEdges.front()!.startIndex, nextExpectedStartIdx, outEdges.size());
+    // #v-ifdef DEV
+    // Assert that we have a fully connected loop
+    assert(
+      outEdges.empty() || outEdges.front()!.startIndex === nextExpectedStartIdx,
+      "Output edges do not form a fully connected loop"
+    );
+    // #v-endif
+
+    // When we start with two triangles facing away from each other and adding a point that is on the plane,
+    // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
+    // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
+    return outEdges.size() >= 3;
+  }
+
+  // Add a new point to the convex hull
+  addPoint(inFacingTriangle: Triangle, inIdx: number, inClosestDistSq: number, outTriangles: NewTriangles) {
+    // #v-ifdef DEV
+    if (!this.positions) {
+      throw new Error("Positions array is not initialized");
+    }
+    // #v-endif
+
+    // Get position
+    const pos = this.positions.values[inIdx];
+
+    // Find edge of convex hull of triangles that are not facing the new vertex
+    const edges = this.edges;
+    edges.clear();
+
+    if (!this.findEdge(inFacingTriangle, pos, edges)) {
+      return false;
+    }
+
+    // Create new triangles
+    const numEdges = edges.size();
+    for (let i = 0; i < numEdges; ++i) {
+      // Create new triangle
+      const nt = this.createTriangle(edges.values[i].startIndex, edges.values[(i + 1) % numEdges].startIndex, inIdx);
+
+      if (!nt) {
+        return false;
+      }
+
+      outTriangles.pushBack(nt);
+
+      // Check if we need to put this triangle in the priority queue
+      if (
+        (nt.closestPointInterior && nt.closestLengthSq < inClosestDistSq) || // For the main algorithm
+        nt.closestLengthSq < 0.0 // For when the origin is not inside the hull yet
+      ) {
+        this.triangleQueue.pushBack(nt);
+      }
+    }
+
+    // Link edges
+    for (let i = 0; i < numEdges; ++i) {
+      linkTriangle(outTriangles.values[i]!, 0, edges.values[i].neighbourTriangle!, edges.values[i].neighbourEdge);
+      linkTriangle(outTriangles.values[i]!, 1, outTriangles.values[(i + 1) % numEdges]!, 2);
+    }
+
+    return true;
+  }
+}
+
+const vectorAB = /*@__PURE__*/ Vec3.create();

package/src/collision/epa/StaticArray.ts

@@ -0,0 +1,154 @@
+interface Copyable<T> {
+  copy(value: T): void;
+}
+
+export class StaticArray<T extends Copyable<any>> {
+  values: T[];
+  _size: number;
+
+  constructor(capacity: number) {
+    this.values = new Array(capacity).fill(null);
+    this._size = 0;
+  }
+
+  // initialize(values: T[]) {
+  //   if (values.length > this.values.length) {
+  //     throw new Error("values array exceeds static array capacity");
+  //   }
+
+  //   this._size = Math.min(values.length, this.values.length);
+  //   for (let i = 0; i < this._size; i++) {
+  //     this.values[i] = values[i];
+  //   }
+  //   return this;
+  // }
+
+  clear() {
+    this._size = 0;
+  }
+
+  /**
+   * returns the size of the static array.
+   */
+  size() {
+    return this._size;
+  }
+
+  /**
+   * returns whether the static array is empty.
+   */
+  empty() {
+    return this._size === 0;
+  }
+
+  /**
+   * returns the first element of the static array.
+   */
+  front() {
+    if (this._size === 0) {
+      return null;
+    }
+    return this.values[0];
+  }
+
+  /**
+   * returns the last element of the static array.
+   */
+  back() {
+    if (this._size === 0) {
+      return null;
+    }
+    return this.values[this._size - 1];
+  }
+
+  /**
+   * returns the beginning index of the static array.
+   */
+  begin() {
+    return 0;
+  }
+
+  /**
+   * returns the end index of the static array.
+   */
+  end() {
+    return this._size;
+  }
+
+  /**
+   * returns the actual array of the static array.
+   */
+  data() {
+    return this.values;
+  }
+
+  /**
+   * Pushes a value to the end of the static array.
+   */
+  pushBack(value: T) {
+    if (this._size >= this.values.length) {
+      debugger;
+      throw new Error("cannot push to a full array");
+    }
+    this.values[this._size++] = value;
+  }
+
+  pushBackCopy(value: T) {
+    if (this._size >= this.values.length) {
+      throw new Error("cannot push to a full array");
+    }
+    // Assuming T is a type that can be copied
+    this.values[this._size++].copy(value);
+  }
+
+  /**
+   * Removes the last value from the static array and returns it.
+   */
+  popBack() {
+    if (this._size === 0) {
+      throw new Error("cannot pop from an empty array");
+    }
+    const value = this.values[--this._size];
+    // this.values[this._size] = null; // clear the slot
+    return value;
+  }
+
+  popBackCopy() {
+    if (this._size === 0) {
+      throw new Error("cannot pop from an empty array");
+    }
+    const value = this.values[--this._size];
+    // this.values[this._size] = null; // clear the slot
+    return value;
+  }
+
+  at(index: number) {
+    if (index >= this._size) {
+      throw new Error("index out of range");
+    }
+    return this.values[index] as T;
+  }
+
+  set(index: number, value: T) {
+    if (index >= this._size) {
+      throw new Error("index out of range");
+    }
+    this.values[index] = value;
+  }
+
+  [Symbol.iterator](): Iterator<T> {
+    let index = 0;
+    const size = this._size;
+    const values = this.values;
+
+    return {
+      next(): IteratorResult<T> {
+        if (index < size) {
+          return { value: values[index++] as T, done: false };
+        } else {
+          return { value: undefined, done: true };
+        }
+      },
+    };
+  }
+}

package/src/collision/epa/TriangleFactory.ts

@@ -0,0 +1,32 @@
+import { initTriangle, Triangle } from "./structs";
+import { ArrayPool } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+
+export class TriangleFactory {
+  pool: ArrayPool<Triangle>;
+  minTriangleArea: number;
+  barycentricEpsilon: number;
+
+  constructor(maxTriangles: number, minTriangleArea: number, barycentricEpsilon: number) {
+    this.pool = new ArrayPool(Triangle, maxTriangles);
+    this.minTriangleArea = minTriangleArea;
+    this.barycentricEpsilon = barycentricEpsilon;
+  }
+
+  clear() {
+    this.pool.clear();
+  }
+
+  createTriangle(inIdx0: number, inIdx1: number, inIdx2: number, inPositions: Vec3[]) {
+    const triangle = this.pool.create();
+    if (!triangle) {
+      return null; // Pool is exhausted
+    }
+    initTriangle(triangle, inIdx0, inIdx1, inIdx2, inPositions, this.minTriangleArea, this.barycentricEpsilon);
+    return triangle;
+  }
+
+  freeTriangle(inT: Triangle) {
+    this.pool.remove(inT);
+  }
+}

package/src/collision/epa/arrays.ts

@@ -0,0 +1,99 @@
+import { PoolClass } from "monomorph";
+import { Vec3 } from "../../math/vec3";
+import { binaryHeapPop, binaryHeapPush } from "./binaryHeap";
+import { StaticArray } from "./StaticArray";
+import { Edge, Triangle } from "./structs";
+
+class PointsBase extends StaticArray<Vec3> {
+  pool: PoolClass<Vec3>;
+  constructor(capacity: number) {
+    super(capacity);
+    this.pool = new Vec3.Pool(capacity);
+    for (let i = 0; i < capacity; i++) {
+      const instance = this.pool.create();
+      this.pushBack(instance);
+    }
+    this.clear();
+  }
+}
+
+export class Points extends PointsBase {
+  getSizeRef() {
+    return this;
+  }
+}
+
+export class Edges extends StaticArray<Edge> {
+  pool: PoolClass<Edge>;
+  constructor(capacity: number) {
+    super(capacity);
+    this.pool = new Edge.Pool(capacity);
+    for (let i = 0; i < capacity; i++) {
+      const instance = this.pool.create();
+      this.pushBack(instance);
+    }
+    this.clear();
+  }
+}
+
+export class NewTriangles extends StaticArray<Triangle> {
+  constructor(capacity: number) {
+    super(capacity);
+  }
+}
+
+export class Triangles extends StaticArray<Triangle> {
+  constructor(capacity: number) {
+    super(capacity);
+  }
+}
+
+/**
+ * Specialized triangles list that keeps them sorted on closest distance to origin
+ */
+export class TriangleQueue extends Triangles {
+  constructor(capacity: number) {
+    super(capacity);
+  }
+
+  /**
+   * Function to sort triangles on closest distance to origin
+   */
+  static triangleSorter(inT1: Triangle, inT2: Triangle) {
+    return inT1!.closestLengthSq > inT2!.closestLengthSq;
+  }
+
+  /**
+   * Add triangle to the list
+   */
+  pushBack(inT: Triangle) {
+    // Add to base
+    super.pushBack(inT);
+
+    // Mark in queue
+    inT!.inQueue = true;
+
+    // Resort heap
+    binaryHeapPush(this.values, this.begin(), this.end(), TriangleQueue.triangleSorter);
+  }
+
+  /**
+   * Peek the next closest triangle without removing it
+   */
+  peekClosest() {
+    return this.front();
+  }
+
+  /**
+   * Get next closest triangle
+   */
+  popClosest() {
+    // Move closest to end
+    binaryHeapPop(this.values, this.begin(), this.end(), TriangleQueue.triangleSorter);
+
+    // Remove last triangle
+    const t = this.back();
+    this.popBack();
+    return t;
+  }
+}