@perplexdotgg/bounce 1.0.0

package/src/builders/ConvexHullBuilder2d.ts

@@ -0,0 +1,344 @@
+import { PoolClass } from "monomorph";
+import { Vec3 } from "../math/vec3";
+import { squared } from "../math/scalar";
+
+// TODO: this may need to change, not sure if value is too large
+const floatEpsilon = 1e-5;
+
+const normal = /*@__PURE__*/ Vec3.create();
+const center = /*@__PURE__*/ Vec3.create();
+const tempVector = /*@__PURE__*/ Vec3.create();
+
+const p1 = /*@__PURE__*/ Vec3.create();
+const p2 = /*@__PURE__*/ Vec3.create();
+
+const edge = /*@__PURE__*/ Vec3.create();
+const point = /*@__PURE__*/ Vec3.create();
+
+const maxComponents = /*@__PURE__*/ Vec3.create();
+const currentVertex = /*@__PURE__*/ Vec3.create();
+const vertexA = /*@__PURE__*/ Vec3.create();
+const vertexB = /*@__PURE__*/ Vec3.create();
+const vertexC = /*@__PURE__*/ Vec3.create();
+const vectorAB = /*@__PURE__*/ Vec3.create();
+const vectorAC = /*@__PURE__*/ Vec3.create();
+const crossAB_AC = /*@__PURE__*/ Vec3.create();
+const furthestPoint = /*@__PURE__*/ Vec3.create();
+
+/// Result enum that indicates how the hull got created
+export const enum ConvexHullBuilder2dResult {
+  Success, ///< Hull building finished successfully
+  MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+}
+
+type Tuple3 = [number, number, number];
+
+class ConvexHull2dEdge {
+  normal: Tuple3;
+  center: Tuple3;
+  conflictList: number[];
+  prevEdge?: ConvexHull2dEdge;
+  nextEdge?: ConvexHull2dEdge;
+  startIndex: number;
+  furthestPointDistanceSquared: number = 0;
+
+  constructor(startIndex: number) {
+    this.normal = [0, 0, 0];
+    this.center = [0, 0, 0];
+    this.conflictList = [];
+    this.startIndex = startIndex;
+  }
+
+  isFacing(position: Vec3): boolean {
+    normal.x = this.normal[0];
+    normal.y = this.normal[1];
+    normal.z = this.normal[2];
+
+    center.x = this.center[0];
+    center.y = this.center[1];
+    center.z = this.center[2];
+
+    tempVector.subtractVectors(position, center);
+    return normal.dot(tempVector) > 0;
+  }
+
+  calculateNormalAndCenter(polyhedron: PoolClass<Vec3>): void {
+    p1.copy(polyhedron.array[this.startIndex]);
+    p2.copy(polyhedron.array[this.nextEdge!.startIndex]);
+
+    // Center of edge
+    center.addVectors(p1, p2);
+    center.scale(0.5);
+
+    // Create outward pointing normal.
+    // We have two choices for the normal (which satisfies normal . edge = 0):
+    // normal1 = (-edge.y, edge.x, 0)
+    // normal2 = (edge.y, -edge.x, 0)
+    // We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition.
+    edge.subtractVectors(p2, p1);
+    normal.x = edge.y;
+    normal.y = -edge.x;
+    normal.z = 0;
+
+    this.normal[0] = normal.x;
+    this.normal[1] = normal.y;
+    this.normal[2] = normal.z;
+
+    this.center[0] = center.x;
+    this.center[1] = center.y;
+    this.center[2] = center.z;
+  }
+}
+
+export class ConvexHullBuilder2d {
+  firstEdge?: ConvexHull2dEdge;
+  numEdges: number = 0;
+
+  freeEdges(): void {
+    this.firstEdge = undefined;
+    this.numEdges = 0;
+  }
+
+  validateEdges(): void {
+    if (this.firstEdge === undefined) {
+      if (this.numEdges !== 0) {
+        throw new Error("mNumEdges is not 0");
+      }
+      return;
+    }
+
+    let count = 0;
+
+    let edge = this.firstEdge;
+    do {
+      // Validate connectivity
+      if (edge.nextEdge!.prevEdge !== edge) {
+        throw new Error("edge->mNextEdge->mPrevEdge != edge");
+      }
+
+      if (edge.prevEdge!.nextEdge !== edge) {
+        throw new Error("edge->mPrevEdge->mNextEdge != edge");
+      }
+
+      ++count;
+      edge = edge.nextEdge!;
+    } while (edge !== this.firstEdge);
+
+    // Validate that count matches
+    if (count !== this.numEdges) {
+      throw new Error("count != mNumEdges");
+    }
+  }
+
+  assignPointToEdge(polyhedron: PoolClass<Vec3>, positionIndex: number, edges: ConvexHull2dEdge[]): void {
+    point.copy(polyhedron.array[positionIndex]);
+
+    let bestEdge: ConvexHull2dEdge | undefined = undefined;
+    let bestDistanceSquared = 0;
+
+    // Test against all edges
+    for (const edge of edges) {
+      // Determine distance to edge
+      normal.x = edge.normal[0];
+      normal.y = edge.normal[1];
+      normal.z = edge.normal[2];
+
+      center.x = edge.center[0];
+      center.y = edge.center[1];
+      center.z = edge.center[2];
+
+      tempVector.subtractVectors(point, center);
+      const dot = normal.dot(tempVector);
+      if (dot > 0.0) {
+        const distanceSquared = (dot * dot) / normal.squaredLength();
+        if (distanceSquared > bestDistanceSquared) {
+          bestEdge = edge;
+          bestDistanceSquared = distanceSquared;
+        }
+      }
+    }
+
+    // If this point is in front of the edge, add it to the conflict list
+    if (bestEdge) {
+      if (bestDistanceSquared > bestEdge.furthestPointDistanceSquared) {
+        // This point is futher away than any others, update the distance and add point as last point
+        bestEdge.furthestPointDistanceSquared = bestDistanceSquared;
+        bestEdge.conflictList.push(positionIndex);
+      } else {
+        // Not the furthest point, add it as the before last point
+        bestEdge.conflictList.push(bestEdge.conflictList[bestEdge.conflictList.length - 1]);
+        bestEdge.conflictList[bestEdge.conflictList.length - 2] = positionIndex;
+      }
+    }
+  }
+
+  buildConvexHull(
+    outEdges: number[],
+    index1: number,
+    index2: number,
+    index3: number,
+    polyhedron: PoolClass<Vec3>,
+    tolerance: number,
+    maxVertexCount: number
+  ): ConvexHullBuilder2dResult {
+    // Clear any leftovers
+    this.freeEdges();
+    outEdges.length = 0;
+
+    // Reset flag
+    let result: ConvexHullBuilder2dResult = ConvexHullBuilder2dResult.Success;
+
+    // Determine a suitable tolerance for detecting that points are colinear
+    // Formula as per: Implementing Quickhull - Dirk Gregorius.
+    maxComponents.zero();
+    for (let i = 0; i < polyhedron.length; i++) {
+      currentVertex.copy(polyhedron.array[i]);
+      maxComponents.x = Math.max(maxComponents.x, Math.abs(currentVertex.x));
+      maxComponents.y = Math.max(maxComponents.y, Math.abs(currentVertex.y));
+      // maxComponents.z = Math.max(maxComponents.z, Math.abs(currentVertex.z));
+    }
+
+    const colinearToleranceSquared = squared(2.0 * floatEpsilon * (maxComponents.x + maxComponents.y));
+
+    // Increase desired tolerance if accuracy doesn't allow it
+    const toleranceSquared = Math.max(colinearToleranceSquared, squared(tolerance));
+
+    // Start with the initial indices in counter clockwise order
+    vertexA.copy(polyhedron.array[index1]);
+    vertexB.copy(polyhedron.array[index2]);
+    vertexC.copy(polyhedron.array[index3]);
+    vectorAB.subtractVectors(vertexB, vertexA);
+    vectorAC.subtractVectors(vertexC, vertexA);
+    crossAB_AC.crossVectors(vectorAB, vectorAC);
+    const z = crossAB_AC.z;
+    if (z < 0.0) {
+      const tempIndex = index1;
+      index1 = index2;
+      index2 = tempIndex;
+    }
+
+    // Create and link edges
+    const e1 = new ConvexHull2dEdge(index1);
+    const e2 = new ConvexHull2dEdge(index2);
+    const e3 = new ConvexHull2dEdge(index3);
+    e1.nextEdge = e2;
+    e1.prevEdge = e3;
+    e2.nextEdge = e3;
+    e2.prevEdge = e1;
+    e3.nextEdge = e1;
+    e3.prevEdge = e2;
+    this.firstEdge = e1;
+    this.numEdges = 3;
+
+    // Build the initial conflict lists
+    const edges: ConvexHull2dEdge[] = [e1, e2, e3];
+    for (const edge of edges) {
+      edge.calculateNormalAndCenter(polyhedron);
+    }
+
+    for (let index = 0; index < polyhedron.length; ++index) {
+      if (index === index1 || index === index2 || index === index3) {
+        continue;
+      }
+
+      this.assignPointToEdge(polyhedron, index, edges);
+    }
+
+    // Add the remaining points to the hull
+    while (true) {
+      // Check if we've reached the max amount of vertices that are allowed
+      if (this.numEdges >= maxVertexCount) {
+        result = ConvexHullBuilder2dResult.MaxVerticesReached;
+        break;
+      }
+
+      // Find the edge with the furthest point on it
+      let edgeWithFurthestPoint: ConvexHull2dEdge | undefined = undefined;
+      let furthestDistanceSquared = 0.0;
+      let edge: ConvexHull2dEdge = this.firstEdge;
+
+      do {
+        if (edge.furthestPointDistanceSquared > furthestDistanceSquared) {
+          furthestDistanceSquared = edge.furthestPointDistanceSquared;
+          edgeWithFurthestPoint = edge;
+        }
+        edge = edge.nextEdge!;
+      } while (edge !== this.firstEdge);
+
+      // If there is none closer than our tolerance value, we're done
+      if (!edgeWithFurthestPoint || furthestDistanceSquared < toleranceSquared) {
+        break;
+      }
+
+      // Take the furthest point
+      const furthestPointIndex = edgeWithFurthestPoint.conflictList.pop()!;
+      furthestPoint.copy(polyhedron.array[furthestPointIndex]);
+
+      // Find the horizon of edges that need to be removed
+      let firstEdge = edgeWithFurthestPoint;
+      do {
+        const prev = firstEdge.prevEdge!;
+        if (!prev.isFacing(furthestPoint)) {
+          break;
+        }
+        firstEdge = prev;
+      } while (firstEdge !== edgeWithFurthestPoint);
+
+      let lastEdge = edgeWithFurthestPoint;
+      do {
+        const next = lastEdge.nextEdge!;
+        if (!next.isFacing(furthestPoint)) {
+          break;
+        }
+        lastEdge = next;
+      } while (lastEdge !== edgeWithFurthestPoint);
+
+      // Create new edges
+      const e1 = new ConvexHull2dEdge(firstEdge.startIndex);
+      const e2 = new ConvexHull2dEdge(furthestPointIndex);
+      e1.nextEdge = e2;
+      e1.prevEdge = firstEdge.prevEdge;
+      e2.prevEdge = e1;
+      e2.nextEdge = lastEdge.nextEdge;
+      e1.prevEdge!.nextEdge = e1;
+      e2.nextEdge!.prevEdge = e2;
+      this.firstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge
+      this.numEdges += 2;
+
+      // Calculate normals
+      const newEdges: ConvexHull2dEdge[] = [e1, e2];
+      for (const newEdge of newEdges) {
+        newEdge.calculateNormalAndCenter(polyhedron);
+      }
+
+      // Delete the old edges
+      while (true) {
+        const next = firstEdge.nextEdge;
+
+        // Redistribute points in conflict list
+        for (const index of firstEdge.conflictList) {
+          this.assignPointToEdge(polyhedron, index, newEdges);
+        }
+
+        // Delete the old edge
+        // delete first_edge; TODO: probably not needed in TS
+        --this.numEdges;
+
+        if (firstEdge === lastEdge) {
+          break;
+        }
+        firstEdge = next!;
+      }
+    }
+
+    // Convert the edge list to a list of indices
+    outEdges.length = 0;
+    let edge = this.firstEdge;
+    do {
+      outEdges.push(edge.startIndex);
+      edge = edge.nextEdge!;
+    } while (edge !== this.firstEdge);
+
+    return result;
+  }
+}