@perplexdotgg/bounce 1.0.0

package/src/builders/ConvexHullBuilder3d.ts

@@ -0,0 +1,1689 @@
+import { PoolClass } from "monomorph";
+import { ClosestPointResult } from "../collision/closestPoints/closestPoints";
+import { Vec3 } from "../math/vec3";
+import { degreesToRadians, squared } from "../math/scalar";
+import { ConvexHullBuilder2d, ConvexHullBuilder2dResult } from "./ConvexHullBuilder2d";
+import { computeClosestPointOnLine } from "../collision/closestPoints/computeClosestPointOnLine";
+
+// TODO: this may need to change, not sure if value is too large
+const floatEpsilon = 1e-5;
+
+const minTriangleAreaSquared = 1e-12;
+
+type Tuple3 = [number, number, number];
+
+const calculateNormalAndCentroidy0 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroidy1 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroidy2 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroide0 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroide1 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroide2 = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroidshortestEdgeNormal = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroidcentroid = /*@__PURE__*/ Vec3.create();
+const calculateNormalAndCentroidnormal = /*@__PURE__*/ Vec3.create();
+
+const isFacingcentroid = /*@__PURE__*/ Vec3.create();
+const isFacingnormal = /*@__PURE__*/ Vec3.create();
+const isFacingvectorFromCentroid = /*@__PURE__*/ Vec3.create();
+
+const getFaceForPointcentroid = /*@__PURE__*/ Vec3.create();
+const getFaceForPointnormal = /*@__PURE__*/ Vec3.create();
+const getFaceForPointtempVector = /*@__PURE__*/ Vec3.create();
+
+const getDistanceToEdgeSquaredp1 = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredp2 = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredvector12 = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredtempVector = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquarednormal = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredp1ToPoint = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredclosest = /*@__PURE__*/ ClosestPointResult.create();
+const getDistanceToEdgeSquaredpointToP1 = /*@__PURE__*/ Vec3.create();
+const getDistanceToEdgeSquaredpointToP2 = /*@__PURE__*/ Vec3.create();
+
+const assignPointToFacepoint = /*@__PURE__*/ Vec3.create();
+
+const determineCoplanarDistancecurrentVertex = /*@__PURE__*/ Vec3.create();
+const determineCoplanarDistancemaxComponents = /*@__PURE__*/ Vec3.create();
+
+const buildConvexHullvertexA = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvertexB = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvertexC = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvertexD = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvectorCA = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvectorCB = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvectorAB = /*@__PURE__*/ Vec3.create();
+const buildConvexHullvectorAC = /*@__PURE__*/ Vec3.create();
+const buildConvexHullcrossCA_CB = /*@__PURE__*/ Vec3.create();
+const buildConvexHullinitialPlaneNormal = /*@__PURE__*/ Vec3.create();
+const buildConvexHullinitialPlaneCentroid = /*@__PURE__*/ Vec3.create();
+const buildConvexHulltempVector = /*@__PURE__*/ Vec3.create();
+const buildConvexHullbase1 = /*@__PURE__*/ Vec3.create();
+const buildConvexHullbase2 = /*@__PURE__*/ Vec3.create();
+
+const addPointposition = /*@__PURE__*/ Vec3.create();
+
+const mergeDegenerateFacenormal = /*@__PURE__*/ Vec3.create();
+const mergeDegenerateFacep1 = /*@__PURE__*/ Vec3.create();
+const mergeDegenerateFacep2 = /*@__PURE__*/ Vec3.create();
+
+const mergeCoplanarOrConcaveFacesdeltaCentroid = /*@__PURE__*/ Vec3.create();
+const mergeCoplanarOrConcaveFacesinFaceNormal = /*@__PURE__*/ Vec3.create();
+const mergeCoplanarOrConcaveFacesinFaceCentroid = /*@__PURE__*/ Vec3.create();
+const mergeCoplanarOrConcaveFacesotherFaceNormal = /*@__PURE__*/ Vec3.create();
+const mergeCoplanarOrConcaveFacesotherFaceCentroid = /*@__PURE__*/ Vec3.create();
+
+const getCenterOfMassAndVolumevertexA = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevertexB = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevertexC = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevertexD = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevectorDA = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevectorDB = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumevectorDC = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumecrossDB_DC = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumecentroid = /*@__PURE__*/ Vec3.create();
+const getCenterOfMassAndVolumecenterOfMassTetrahedron = /*@__PURE__*/ Vec3.create();
+
+const determineMaxError = /*@__PURE__*/ Vec3.create();
+const determineMaxErrornormal = /*@__PURE__*/ Vec3.create();
+const determineMaxErrorcentroid = /*@__PURE__*/ Vec3.create();
+const determineMaxErrortempVector = /*@__PURE__*/ Vec3.create();
+
+const faceNormal = /*@__PURE__*/ Vec3.create();
+const neighborNormal = /*@__PURE__*/ Vec3.create();
+
+export const enum ConvexHullBuilderResult {
+  Success, ///< Hull building finished successfully
+  MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+  TooFewPoints, ///< Too few points to create a hull
+  TooFewFaces, ///< Too few faces in the created hull (signifies precision errors during building)
+  Degenerate, ///< Degenerate hull detected
+}
+
+class ConvexHullBuilderCoplanar {
+  positionIndex: number;
+  distanceSquared: number;
+
+  constructor(positionIndex: number, distanceSquared: number) {
+    this.positionIndex = positionIndex;
+    this.distanceSquared = distanceSquared;
+  }
+}
+
+class ConvexHullBuilderEdge {
+  face: ConvexHullBuilderFace;
+  nextEdge?: ConvexHullBuilderEdge;
+  neighbourEdge?: ConvexHullBuilderEdge;
+  startIndex: number;
+  // isFirst: boolean = false;
+
+  constructor(face: ConvexHullBuilderFace, startIndex: number) {
+    this.face = face;
+    this.startIndex = startIndex;
+  }
+
+  getPreviousEdge() {
+    let prevEdge: ConvexHullBuilderEdge | undefined = this;
+    while (prevEdge?.nextEdge && prevEdge.nextEdge !== this) {
+      prevEdge = prevEdge.nextEdge;
+    }
+    return prevEdge;
+  }
+}
+
+class ConvexHullBuilderFullEdge {
+  startIndex: number;
+  endIndex: number;
+  neighbourEdge: ConvexHullBuilderEdge;
+
+  constructor(startIndex: number, endIndex: number, neighbourEdge: ConvexHullBuilderEdge) {
+    this.startIndex = startIndex;
+    this.endIndex = endIndex;
+    this.neighbourEdge = neighbourEdge;
+  }
+}
+
+export class ConvexHullBuilderFace {
+  normal: Tuple3;
+  centroid: Tuple3;
+  conflictList: number[];
+  firstEdge?: ConvexHullBuilderEdge;
+  furthestPointDistanceSquared: number = 0;
+  removed: boolean = false;
+
+  indices: Tuple3;
+
+  constructor() {
+    this.normal = [0, 0, 0];
+    this.centroid = [0, 0, 0];
+    this.conflictList = [];
+    this.indices = [0, 0, 0];
+  }
+
+  copy(face: ConvexHullBuilderFace) {
+    this.normal[0] = face.normal[0];
+    this.normal[1] = face.normal[1];
+    this.normal[2] = face.normal[2];
+    this.centroid[0] = face.centroid[0];
+    this.centroid[1] = face.centroid[1];
+    this.centroid[2] = face.centroid[2];
+    this.conflictList = face.conflictList.slice();
+    this.firstEdge = face.firstEdge;
+    this.furthestPointDistanceSquared = face.furthestPointDistanceSquared;
+    this.removed = face.removed;
+    this.indices[0] = face.indices[0];
+    this.indices[1] = face.indices[1];
+    this.indices[2] = face.indices[2];
+  }
+
+  initialize(index0: number, index1: number, index2: number, polyhedron: PoolClass<Vec3>) {
+    if (index0 === index1 || index0 === index2 || index1 === index2) {
+      throw new Error("Indices must be unique");
+    }
+
+    // Create 3 edges
+    const edge0 = new ConvexHullBuilderEdge(this, index0);
+    const edge1 = new ConvexHullBuilderEdge(this, index1);
+    const edge2 = new ConvexHullBuilderEdge(this, index2);
+
+    // Link edges
+    edge0.nextEdge = edge1;
+    edge1.nextEdge = edge2;
+    edge2.nextEdge = edge0;
+    this.firstEdge = edge0;
+
+    this.calculateNormalAndCentroid(polyhedron);
+
+    this.indices[0] = index0;
+    this.indices[1] = index1;
+    this.indices[2] = index2;
+  }
+
+  calculateNormalAndCentroid(polyhedron: PoolClass<Vec3>) {
+    // Get point that we use to construct a triangle fan
+    let edge = this.firstEdge!;
+    calculateNormalAndCentroidy0.copy(polyhedron.array[edge.startIndex]);
+
+    // Get the 2nd point
+    edge = edge.nextEdge!;
+    calculateNormalAndCentroidy1.copy(polyhedron.array[edge.startIndex]);
+
+    // Start accumulating the centroid
+    // centroid.zero();
+    calculateNormalAndCentroidcentroid.addVectors(calculateNormalAndCentroidy0, calculateNormalAndCentroidy1);
+    let n = 2;
+
+    // Start accumulating the normal
+    calculateNormalAndCentroidnormal.zero();
+
+    // Loop over remaining edges accumulating normals in a triangle fan fashion
+    for (edge = edge.nextEdge!; edge !== this.firstEdge; edge = edge.nextEdge!) {
+      // Get the 3rd point
+      calculateNormalAndCentroidy2.copy(polyhedron.array[edge.startIndex]);
+
+      // Calculate edges (counter clockwise)
+      calculateNormalAndCentroide0.subtractVectors(calculateNormalAndCentroidy1, calculateNormalAndCentroidy0);
+      calculateNormalAndCentroide1.subtractVectors(calculateNormalAndCentroidy2, calculateNormalAndCentroidy1);
+      calculateNormalAndCentroide2.subtractVectors(calculateNormalAndCentroidy0, calculateNormalAndCentroidy2);
+
+      // The best normal is calculated by using the two shortest edges
+      // See: https://box2d.org/posts/2014/01/troublesome-triangle/
+      // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the others must have roughly the same length).
+      // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+
+      if (
+        calculateNormalAndCentroide1.dot(calculateNormalAndCentroide1) <
+        calculateNormalAndCentroide2.dot(calculateNormalAndCentroide2)
+      ) {
+        calculateNormalAndCentroidshortestEdgeNormal.crossVectors(
+          calculateNormalAndCentroide0,
+          calculateNormalAndCentroide1
+        );
+      } else {
+        calculateNormalAndCentroidshortestEdgeNormal.crossVectors(
+          calculateNormalAndCentroide2,
+          calculateNormalAndCentroide0
+        );
+      }
+
+      // Accumulate normal
+      calculateNormalAndCentroidnormal.addVectors(
+        calculateNormalAndCentroidnormal,
+        calculateNormalAndCentroidshortestEdgeNormal
+      );
+
+      // Accumulate centroid
+      calculateNormalAndCentroidcentroid.addVectors(calculateNormalAndCentroidcentroid, calculateNormalAndCentroidy2);
+      n++;
+
+      // Update y1 for next triangle
+      calculateNormalAndCentroidy1.copy(calculateNormalAndCentroidy2);
+    }
+
+    // Finalize normal, TODO: this is not done in the reference implementation
+    calculateNormalAndCentroidnormal.normalizeVector(calculateNormalAndCentroidnormal);
+
+    // Finalize centroid
+    calculateNormalAndCentroidcentroid.scaleVector(calculateNormalAndCentroidcentroid, 1 / n);
+
+    // update centroid and normal data
+    this.centroid[0] = calculateNormalAndCentroidcentroid.x;
+    this.centroid[1] = calculateNormalAndCentroidcentroid.y;
+    this.centroid[2] = calculateNormalAndCentroidcentroid.z;
+    this.normal[0] = calculateNormalAndCentroidnormal.x;
+    this.normal[1] = calculateNormalAndCentroidnormal.y;
+    this.normal[2] = calculateNormalAndCentroidnormal.z;
+  }
+
+  isFacing(point: Vec3) {
+    isFacingcentroid.x = this.centroid[0];
+    isFacingcentroid.y = this.centroid[1];
+    isFacingcentroid.z = this.centroid[2];
+    isFacingnormal.x = this.normal[0];
+    isFacingnormal.y = this.normal[1];
+    isFacingnormal.z = this.normal[2];
+
+    isFacingvectorFromCentroid.subtractVectors(point, isFacingcentroid);
+    return isFacingnormal.dot(isFacingvectorFromCentroid) > 0;
+  }
+}
+
+export class ConvexHullBuilder3d {
+  polyhedron?: PoolClass<Vec3>;
+  coplanarList: ConvexHullBuilderCoplanar[];
+  faces: ConvexHullBuilderFace[];
+
+  constructor() {
+    this.coplanarList = [];
+    this.faces = [];
+  }
+
+  freeFaces() {
+    this.faces.length = 0;
+  }
+
+  getFaceForPoint(point: Vec3, faces: ConvexHullBuilderFace[]): [ConvexHullBuilderFace | undefined, number] {
+    let outFace = undefined;
+    let outDistanceSquared = 0.0;
+
+    for (const face of faces) {
+      if (!face.removed) {
+        // Determine distance to face
+        getFaceForPointcentroid.x = face.centroid[0];
+        getFaceForPointcentroid.y = face.centroid[1];
+        getFaceForPointcentroid.z = face.centroid[2];
+        getFaceForPointnormal.x = face.normal[0];
+        getFaceForPointnormal.y = face.normal[1];
+        getFaceForPointnormal.z = face.normal[2];
+        getFaceForPointtempVector.subtractVectors(point, getFaceForPointcentroid);
+        const dot = getFaceForPointnormal.dot(getFaceForPointtempVector);
+        if (dot > 0.0) {
+          const distanceSquared = (dot * dot) / getFaceForPointnormal.squaredLength();
+          if (distanceSquared > outDistanceSquared) {
+            outFace = face;
+            outDistanceSquared = distanceSquared;
+          }
+        }
+      }
+    }
+
+    return [outFace, outDistanceSquared];
+  }
+
+  getDistanceToEdgeSquared(point: Vec3, face: ConvexHullBuilderFace): number {
+    let allInside = true;
+    let edgeDistanceSquared = Infinity;
+
+    // Test if it is inside the edges of the polygon
+    let edge = face.firstEdge!;
+
+    getDistanceToEdgeSquaredp1.copy(this.polyhedron!.array[edge.getPreviousEdge().startIndex]);
+
+    do {
+      getDistanceToEdgeSquaredp2.copy(this.polyhedron!.array[edge.startIndex]);
+
+      getDistanceToEdgeSquarednormal.x = face.normal[0];
+      getDistanceToEdgeSquarednormal.y = face.normal[1];
+      getDistanceToEdgeSquarednormal.z = face.normal[2];
+
+      getDistanceToEdgeSquaredvector12.subtractVectors(getDistanceToEdgeSquaredp2, getDistanceToEdgeSquaredp1);
+      getDistanceToEdgeSquaredp1ToPoint.subtractVectors(point, getDistanceToEdgeSquaredp1);
+      getDistanceToEdgeSquaredtempVector.crossVectors(
+        getDistanceToEdgeSquaredvector12,
+        getDistanceToEdgeSquaredp1ToPoint
+      );
+      if (getDistanceToEdgeSquaredtempVector.dot(getDistanceToEdgeSquarednormal) < 0.0) {
+        // It is outside
+        allInside = false;
+
+        // Measure distance to this edge
+        getDistanceToEdgeSquaredpointToP1.subtractVectors(getDistanceToEdgeSquaredp1, point);
+        getDistanceToEdgeSquaredpointToP2.subtractVectors(getDistanceToEdgeSquaredp2, point);
+        computeClosestPointOnLine(
+          getDistanceToEdgeSquaredclosest,
+          getDistanceToEdgeSquaredpointToP1,
+          getDistanceToEdgeSquaredpointToP2
+        );
+        edgeDistanceSquared = Math.min(edgeDistanceSquared, getDistanceToEdgeSquaredclosest.point.squaredLength());
+      }
+      getDistanceToEdgeSquaredp1.copy(getDistanceToEdgeSquaredp2);
+      edge = edge.nextEdge!;
+    } while (edge !== face.firstEdge);
+
+    return allInside ? 0.0 : edgeDistanceSquared;
+  }
+
+  assignPointToFace(
+    polyhedron: PoolClass<Vec3>,
+    index: number,
+    faces: ConvexHullBuilderFace[],
+    toleranceSquared: number
+  ) {
+    assignPointToFacepoint.copy(polyhedron.array[index]);
+
+    // Find the face for which the point is furthest away
+    const [bestFace, bestDistanceSquared] = this.getFaceForPoint(assignPointToFacepoint, faces);
+
+    if (bestFace) {
+      // Check if this point is within the tolerance margin to the plane
+      if (bestDistanceSquared <= toleranceSquared) {
+        // Check distance to edges
+        const distanceToEdgeSquared = this.getDistanceToEdgeSquared(assignPointToFacepoint, bestFace);
+        if (distanceToEdgeSquared > toleranceSquared) {
+          // Point is outside of the face and too far away to discard
+          this.coplanarList.push(new ConvexHullBuilderCoplanar(index, distanceToEdgeSquared));
+        }
+      } else {
+        // This point is in front of the face, add it to the conflict list
+        if (bestDistanceSquared > bestFace.furthestPointDistanceSquared) {
+          // This point is futher away than any others, update the distance and add point as last point
+          bestFace.furthestPointDistanceSquared = bestDistanceSquared;
+          bestFace.conflictList.push(index);
+        } else {
+          // Not the furthest point, add it as the before last point
+          bestFace.conflictList.push(bestFace.conflictList[bestFace.conflictList.length - 1]);
+          bestFace.conflictList[bestFace.conflictList.length - 2] = index;
+        }
+
+        return true;
+      }
+    }
+
+    return false;
+  }
+
+  determineCoplanarDistance() {
+    // Formula as per: Implementing Quickhull - Dirk Gregorius.
+
+    determineCoplanarDistancemaxComponents.zero();
+
+    for (let i = 0; i < this.polyhedron!.length; ++i) {
+      determineCoplanarDistancecurrentVertex.copy(this.polyhedron!.array[i]);
+      determineCoplanarDistancemaxComponents.x = Math.max(
+        determineCoplanarDistancemaxComponents.x,
+        Math.abs(determineCoplanarDistancecurrentVertex.x)
+      );
+      determineCoplanarDistancemaxComponents.y = Math.max(
+        determineCoplanarDistancemaxComponents.y,
+        Math.abs(determineCoplanarDistancecurrentVertex.y)
+      );
+      determineCoplanarDistancemaxComponents.z = Math.max(
+        determineCoplanarDistancemaxComponents.z,
+        Math.abs(determineCoplanarDistancecurrentVertex.z)
+      );
+    }
+
+    return (
+      3.0 *
+      floatEpsilon *
+      (determineCoplanarDistancemaxComponents.x +
+        determineCoplanarDistancemaxComponents.y +
+        determineCoplanarDistancemaxComponents.z)
+    );
+  }
+
+  getNumberOfVerticesUsed() {
+    const usedVertices = new Set<number>();
+    for (const face of this.faces) {
+      let edge = face.firstEdge!;
+      do {
+        usedVertices.add(edge.startIndex);
+        edge = edge.nextEdge!;
+      } while (edge !== face.firstEdge);
+    }
+    return usedVertices.size;
+  }
+
+  // TODO this is unused/untested, i may have broken it - Hamza
+  containsFace(inIndices: number[]): boolean {
+    for (const face of this.faces) {
+      let edge = face.firstEdge;
+      let index = inIndices.indexOf(edge!.startIndex);
+      // let startIndex = inIndices.indexOf(edge!.startIndex);
+      if (index !== inIndices[inIndices.length - 1]) {
+        let matches = 0;
+        // let currentIndex = startIndex;
+
+        do {
+          // Check if index matches
+          if (index !== edge!.startIndex) {
+            break;
+          }
+
+          // Increment number of matches
+          matches++;
+
+          index++;
+
+          if (index === inIndices[inIndices.length - 1]) {
+            index = inIndices[0];
+          }
+
+          // Next index in list of inIndices
+          // currentIndex = (currentIndex + 1) % inIndices.length;
+
+          // Next edge
+          edge = edge!.nextEdge;
+        } while (edge !== face.firstEdge);
+
+        if (matches === inIndices.length) {
+          return true;
+        }
+      }
+    }
+
+    return false;
+  }
+
+  buildConvexHull(
+    polyhedron: PoolClass<Vec3>,
+    positions2d: PoolClass<Vec3>,
+    tolerance: number,
+    maxVertexCount: number,
+    maxAngleBetweenAdjacentFacesDegrees: number = 1
+  ): ConvexHullBuilderResult {
+    this.polyhedron = polyhedron;
+
+    // TODO: cleanup if needed
+    // Free the faces possibly left over from an earlier hull
+    this.freeFaces();
+
+    // Test that we have at least 3 points
+    if (polyhedron.length < 3) {
+      throw new Error("Need at least 3 points to make a hull");
+      return ConvexHullBuilderResult.TooFewPoints;
+    }
+
+    // Determine a suitable tolerance for detecting that points are coplanar
+    const coplanarToleranceSquared = squared(this.determineCoplanarDistance());
+
+    // Increase desired tolerance if accuracy doesn't allow it
+    const toleranceSquared = Math.max(coplanarToleranceSquared, squared(tolerance));
+
+    // Find point furthest from the origin
+    let index1 = -1;
+    let maxDistanceSquared = -1.0;
+    for (let i = 0; i < polyhedron.length; ++i) {
+      buildConvexHullvertexA.copy(polyhedron.array[i]);
+      let distanceSquared = buildConvexHullvertexA.squaredLength();
+      if (distanceSquared > maxDistanceSquared) {
+        maxDistanceSquared = distanceSquared;
+        index1 = i;
+      }
+    }
+
+    if (index1 < 0) {
+      throw new Error("No vertex found");
+    }
+
+    buildConvexHullvertexA.copy(polyhedron.array[index1]);
+
+    // Find point that is furthest away from this point
+    let index2 = -1;
+    maxDistanceSquared = -1.0;
+    for (let i = 0; i < polyhedron.length; ++i) {
+      if (i === index1) {
+        continue;
+      }
+
+      buildConvexHullvertexB.copy(polyhedron.array[i]);
+      let distanceSquared = buildConvexHullvertexA.squaredDistance(buildConvexHullvertexB);
+      if (distanceSquared > maxDistanceSquared) {
+        maxDistanceSquared = distanceSquared;
+        index2 = i;
+      }
+    }
+
+    if (index2 < 0) {
+      throw new Error("No vertex found");
+    }
+
+    buildConvexHullvertexB.copy(polyhedron.array[index2]);
+
+    // Find point that forms the biggest triangle
+    let index3 = -1;
+    let bestTriangleAreaSquared = -1.0;
+    for (let i = 0; i < polyhedron.length; ++i) {
+      if (i === index1 || i === index2) {
+        continue;
+      }
+
+      buildConvexHullvertexC.copy(polyhedron.array[i]);
+      buildConvexHullvectorCA.subtractVectors(buildConvexHullvertexA, buildConvexHullvertexC);
+      buildConvexHullvectorCB.subtractVectors(buildConvexHullvertexB, buildConvexHullvertexC);
+      buildConvexHullcrossCA_CB.crossVectors(buildConvexHullvectorCA, buildConvexHullvectorCB);
+      const triangleAreaSquared = buildConvexHullcrossCA_CB.squaredLength();
+
+      if (triangleAreaSquared > bestTriangleAreaSquared) {
+        bestTriangleAreaSquared = triangleAreaSquared;
+        index3 = i;
+      }
+    }
+
+    if (index3 < 0) {
+      throw new Error("No vertex found");
+    }
+
+    buildConvexHullvertexC.copy(polyhedron.array[index3]);
+
+    if (bestTriangleAreaSquared < minTriangleAreaSquared) {
+      throw new Error("Could not find a suitable initial triangle because its area was too small");
+      return ConvexHullBuilderResult.Degenerate;
+    }
+
+    // Check if we have only 3 vertices
+    if (polyhedron.length === 3) {
+      // Create two triangles (back to back)
+      const triangle1 = this.createTriangle(index1, index2, index3);
+      const triangle2 = this.createTriangle(index1, index3, index2);
+
+      // Link faces edges
+      this.linkFace(triangle1.firstEdge!, triangle2.firstEdge!.nextEdge!.nextEdge!);
+      this.linkFace(triangle1.firstEdge!.nextEdge!, triangle2.firstEdge!.nextEdge!);
+      this.linkFace(triangle1.firstEdge!.nextEdge!.nextEdge!, triangle2.firstEdge!);
+
+      return ConvexHullBuilderResult.Success;
+    }
+
+    // Find point that forms the biggest tetrahedron
+    buildConvexHullvectorAB.subtractVectors(buildConvexHullvertexB, buildConvexHullvertexA);
+    buildConvexHullvectorAC.subtractVectors(buildConvexHullvertexC, buildConvexHullvertexA);
+
+    buildConvexHullinitialPlaneNormal.crossVectors(buildConvexHullvectorAB, buildConvexHullvectorAC);
+    buildConvexHullinitialPlaneNormal.normalizeVector(buildConvexHullinitialPlaneNormal);
+
+    buildConvexHullinitialPlaneCentroid.addVectors(buildConvexHullvertexA, buildConvexHullvertexB);
+    buildConvexHullinitialPlaneCentroid.addVectors(buildConvexHullinitialPlaneCentroid, buildConvexHullvertexC);
+    buildConvexHullinitialPlaneCentroid.scaleVector(buildConvexHullinitialPlaneCentroid, 1 / 3);
+
+    let index4 = -1;
+    let maxDistance = 0.0;
+    for (let i = 0; i < polyhedron.length; ++i) {
+      if (i === index1 || i === index2 || i === index3) {
+        continue;
+      }
+
+      buildConvexHullvertexD.copy(polyhedron.array[i]);
+      buildConvexHulltempVector.subtractVectors(buildConvexHullvertexD, buildConvexHullinitialPlaneCentroid);
+      const distance = buildConvexHulltempVector.dot(buildConvexHullinitialPlaneNormal);
+      if (Math.abs(distance) > Math.abs(maxDistance)) {
+        maxDistance = distance;
+        index4 = i;
+      }
+    }
+
+    if (index4 < 0) {
+      throw new Error("No vertex found");
+    }
+
+    buildConvexHullvertexD.copy(polyhedron.array[index4]);
+
+    // Check if the hull is coplanar
+    if (squared(maxDistance) <= 25.0 * coplanarToleranceSquared) {
+      // First project all points in 2D space
+      buildConvexHullbase1.computeNormalizedPerpendicular(buildConvexHullinitialPlaneNormal);
+      buildConvexHullbase2.crossVectors(buildConvexHullinitialPlaneNormal, buildConvexHullbase1);
+
+      // const positions2d = this.world.allocate(Polyhedron, {
+      //   maxCount: 1,
+      //   extraSize: 3 * polyhedron.vertices.length,
+      // });
+      // positions2d.add();
+      // const offset2d = positions2d.fullOffset;
+
+      for (let i = 0; i < polyhedron.length; ++i) {
+        buildConvexHulltempVector.copy(polyhedron.array[i]);
+        positions2d.array[i].x = buildConvexHullbase1.dot(buildConvexHulltempVector); // x
+        positions2d.array[i].y = buildConvexHullbase2.dot(buildConvexHulltempVector); // y
+        positions2d.array[i].z = 0.0; // z
+        // positions2d.structsBufferFloat64[offset2d + i * 3 + 0] = base1.dot(tempVector);
+        // positions2d.structsBufferFloat64[offset2d + i * 3 + 1] = base2.dot(tempVector);
+        // positions2d.structsBufferFloat64[offset2d + i * 3 + 2] = 0.0;
+      }
+
+      // Build hull
+      const edges2d: number[] = [];
+      const builder2d = new ConvexHullBuilder2d();
+      const result = builder2d.buildConvexHull(edges2d, index1, index2, index3, positions2d, tolerance, maxVertexCount);
+
+      // Create faces (back to back)
+      const face1 = this.createFace();
+      const face2 = this.createFace();
+
+      // Create edges for face 1
+      const edgesF1: ConvexHullBuilderEdge[] = [];
+      for (const startIndex of edges2d) {
+        const edge = new ConvexHullBuilderEdge(face1, startIndex);
+        if (edgesF1.length === 0) {
+          face1.firstEdge = edge;
+        } else {
+          edgesF1[edgesF1.length - 1].nextEdge = edge;
+        }
+        edgesF1.push(edge);
+      }
+      edgesF1[edgesF1.length - 1].nextEdge = face1.firstEdge;
+
+      // Create edges for face 2
+      const edgesF2: ConvexHullBuilderEdge[] = [];
+      for (let i = edges2d.length - 1; i >= 0; --i) {
+        const edge = new ConvexHullBuilderEdge(face2, edges2d[i]);
+        if (edgesF2.length === 0) {
+          face2.firstEdge = edge;
+        } else {
+          edgesF2[edgesF2.length - 1].nextEdge = edge;
+        }
+        edgesF2.push(edge);
+      }
+      edgesF2[edgesF2.length - 1].nextEdge = face2.firstEdge;
+
+      // Link edges
+      for (let i = 0; i < edges2d.length; ++i) {
+        this.linkFace(edgesF1[i], edgesF2[(2 * edges2d.length - 2 - i) % edges2d.length]);
+      }
+
+      // Calculate the plane for both faces
+      face1.calculateNormalAndCentroid(polyhedron);
+      face2.normal[0] = -face1.normal[0];
+      face2.normal[1] = -face1.normal[1];
+      face2.normal[2] = -face1.normal[2];
+      face2.centroid[0] = face1.centroid[0];
+      face2.centroid[1] = face1.centroid[1];
+      face2.centroid[2] = face1.centroid[2];
+
+      return result === ConvexHullBuilder2dResult.MaxVerticesReached
+        ? ConvexHullBuilderResult.MaxVerticesReached
+        : ConvexHullBuilderResult.Success;
+    }
+
+    // Ensure the planes are facing outwards
+    if (maxDistance < 0.0) {
+      let temp = index2;
+      index2 = index3;
+      index3 = temp;
+    }
+
+    // Create tetrahedron
+    const face1 = this.createTriangle(index1, index2, index4);
+    const face2 = this.createTriangle(index2, index3, index4);
+    const face3 = this.createTriangle(index3, index1, index4);
+    const face4 = this.createTriangle(index1, index3, index2);
+
+    // Link face edges
+    this.linkFace(face1.firstEdge!, face4.firstEdge!.nextEdge!.nextEdge!);
+    this.linkFace(face1.firstEdge!.nextEdge!, face2.firstEdge!.nextEdge!.nextEdge!);
+    this.linkFace(face1.firstEdge!.nextEdge!.nextEdge!, face3.firstEdge!.nextEdge!);
+    this.linkFace(face2.firstEdge!, face4.firstEdge!.nextEdge!);
+    this.linkFace(face2.firstEdge!.nextEdge!, face3.firstEdge!.nextEdge!.nextEdge!);
+    this.linkFace(face3.firstEdge!, face4.firstEdge!);
+
+    // Build the initial conflict lists
+    const faces: ConvexHullBuilderFace[] = [face1, face2, face3, face4];
+    for (let index = 0; index < polyhedron.length; ++index) {
+      if (index !== index1 && index !== index2 && index !== index3 && index !== index4) {
+        this.assignPointToFace(polyhedron, index, faces, toleranceSquared);
+      }
+    }
+
+    // Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull
+    let numVerticesUsed = 4;
+
+    // Loop through the remainder of the points and add them
+    while (true) {
+      // Find the face with the furthest point on it
+      let faceWithFurthestPoint: ConvexHullBuilderFace | undefined = undefined;
+      let furthestDistanceSquared = 0.0;
+      for (const face of this.faces) {
+        if (face.furthestPointDistanceSquared > furthestDistanceSquared) {
+          furthestDistanceSquared = face.furthestPointDistanceSquared;
+          faceWithFurthestPoint = face;
+        }
+      }
+
+      let furthestPointIndex: number | undefined = undefined;
+      if (faceWithFurthestPoint) {
+        // Take the furthest point
+        furthestPointIndex = faceWithFurthestPoint.conflictList.pop();
+      } else if (this.coplanarList.length !== 0) {
+        // Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices)
+        let coplanarList: ConvexHullBuilderCoplanar[] = [];
+        // TODO: is this equivalent to the swap?
+        [this.coplanarList, coplanarList] = [coplanarList, this.coplanarList];
+        // this does a deep copy (not fully recursive, but enough for our purposes)... i believe this is equivalent to the swap here
+        // coplanarList = this.coplanarList.map(coplanar => ({ ...coplanar }));
+
+        let added = false;
+        for (const coplanarObject of coplanarList) {
+          added =
+            this.assignPointToFace(polyhedron, coplanarObject.positionIndex, this.faces, toleranceSquared) || added;
+        }
+
+        // If we were able to assign a point, loop again to pick it up
+        if (added) {
+          continue;
+        }
+
+        // If the coplanar list is empty, there are no points left and we're done
+        if (this.coplanarList.length === 0) {
+          break;
+        }
+
+        do {
+          // Find the vertex that is furthest from the hull
+          let bestIndex = 0;
+          let bestDistanceSquared = this.coplanarList[0].distanceSquared;
+          for (let i = 1; i < this.coplanarList.length; ++i) {
+            const coplanarObject = this.coplanarList[i];
+            if (coplanarObject.distanceSquared > bestDistanceSquared) {
+              bestIndex = i;
+              bestDistanceSquared = coplanarObject.distanceSquared;
+            }
+          }
+
+          // Swap it to the end
+          const temp = this.coplanarList[bestIndex];
+          this.coplanarList[bestIndex] = this.coplanarList[this.coplanarList.length - 1];
+          this.coplanarList[this.coplanarList.length - 1] = temp;
+
+          // Remove it
+          furthestPointIndex = this.coplanarList.pop()?.positionIndex;
+
+          // Find the face for which the point is furthest away
+          if (furthestPointIndex) {
+            buildConvexHulltempVector.copy(polyhedron.array[furthestPointIndex]);
+            [faceWithFurthestPoint, bestDistanceSquared] = this.getFaceForPoint(buildConvexHulltempVector, this.faces);
+          }
+        } while (this.coplanarList.length !== 0 && !faceWithFurthestPoint);
+
+        if (!faceWithFurthestPoint) {
+          break;
+        }
+      } else {
+        // If there are no more vertices, we're done
+        break;
+      }
+
+      // Check if we have a limit on the max vertices that we should produce
+      if (numVerticesUsed >= maxVertexCount) {
+        // Count the actual amount of used vertices (we did not take the removal of any vertices into account)
+        numVerticesUsed = this.getNumberOfVerticesUsed();
+
+        // Check if there are too many
+        if (numVerticesUsed >= maxVertexCount) {
+          return ConvexHullBuilderResult.MaxVerticesReached;
+        }
+      }
+
+      // We're about to add another vertex
+      ++numVerticesUsed;
+
+      // Add the point to the hull
+      const newFaces: ConvexHullBuilderFace[] = [];
+      this.addPoint(faceWithFurthestPoint, furthestPointIndex!, coplanarToleranceSquared, newFaces);
+
+      // Redistribute points on conflict lists belonging to removed faces
+      for (const face of this.faces) {
+        if (face.removed) {
+          for (const index of face.conflictList) {
+            this.assignPointToFace(polyhedron, index, newFaces, toleranceSquared);
+          }
+        }
+      }
+
+      // Permanently delete faces that we removed in AddPoint()
+      this.garbageCollectFaces();
+    }
+
+    // this.mergeSimilarFaces(maxAngleBetweenAdjacentFacesDegrees);
+
+    // Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar.
+    if (this.faces.length < 2) {
+      throw new Error("Too few faces in hull");
+      return ConvexHullBuilderResult.TooFewFaces;
+    }
+
+    return ConvexHullBuilderResult.Success;
+  }
+
+  mergeSimilarFaces(maxAngleBetweenAdjacentFaces: number) {
+    const threshold = Math.cos(degreesToRadians(maxAngleBetweenAdjacentFaces));
+
+    for (const face of this.faces) {
+      if (face.removed) continue;
+      let edge = face.firstEdge!;
+      do {
+        const neighbor = edge.neighbourEdge?.face;
+        if (!neighbor || neighbor.removed) {
+          edge = edge.nextEdge!;
+          continue;
+        }
+
+        faceNormal.fromArray(face.normal);
+        neighborNormal.fromArray(neighbor.normal);
+
+        if (faceNormal.dot(neighborNormal) >= threshold) {
+          this.mergeFaces(edge);
+        }
+
+        edge = edge.nextEdge!;
+      } while (edge !== face.firstEdge);
+    }
+
+    this.garbageCollectFaces();
+  }
+
+  addPoint(
+    inFacingFace: ConvexHullBuilderFace,
+    inIndex: number,
+    inCoplanarToleranceSquared: number,
+    outNewFaces: ConvexHullBuilderFace[]
+  ): void {
+    // Get position
+    addPointposition.copy(this.polyhedron!.array[inIndex]);
+
+    this.validateFaces();
+
+    // Find edge of convex hull of faces that are not facing the new vertex
+    const edges: ConvexHullBuilderFullEdge[] = [];
+    this.findEdge(inFacingFace, addPointposition, edges);
+    if (edges.length < 3) {
+      // debugger;
+      throw new Error("Not enough edges found");
+    }
+
+    // Create new faces
+    outNewFaces.length = 0;
+    for (const edge of edges) {
+      if (edge.startIndex === edge.endIndex) {
+        throw new Error("Start and end index are the same");
+      }
+
+      const face = this.createTriangle(edge.startIndex, edge.endIndex, inIndex);
+      outNewFaces.push(face);
+    }
+
+    // Link edges
+    for (let i = 0; i < outNewFaces.length; ++i) {
+      // debugger;
+      this.linkFace(outNewFaces[i].firstEdge!, edges[i].neighbourEdge!);
+      this.linkFace(
+        outNewFaces[i].firstEdge!.nextEdge!,
+        outNewFaces[(i + 1) % outNewFaces.length].firstEdge!.nextEdge!.nextEdge!
+      );
+    }
+
+    // Loop on faces that were modified until nothing needs to be checked anymore
+    const affectedFaces: ConvexHullBuilderFace[] = [...outNewFaces];
+    while (affectedFaces.length !== 0) {
+      // Take the next face
+      const face = affectedFaces.pop()!;
+
+      if (face.removed) {
+        continue;
+      }
+
+      // Merge with neighbour if this is a degenerate face
+      this.mergeDegenerateFace(face, affectedFaces);
+
+      // Merge with coplanar neighbours (or when the neighbour forms a concave edge)
+      if (face.removed) {
+        continue;
+      }
+      this.mergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSquared, affectedFaces);
+    }
+
+    this.validateFaces();
+  }
+
+  garbageCollectFaces(): void {
+    for (let i = this.faces.length - 1; i >= 0; --i) {
+      const face = this.faces[i];
+      if (face.removed) {
+        this.freeFace(face);
+        this.faces.splice(i, 1);
+      }
+    }
+  }
+
+  createFace(): ConvexHullBuilderFace {
+    // Call provider to create face
+    const face = new ConvexHullBuilderFace();
+
+    // Add to list
+    this.faces.push(face);
+    return face;
+  }
+
+  createTriangle(index0: number, index1: number, index2: number) {
+    const face = this.createFace();
+    face.initialize(index0, index1, index2, this.polyhedron!);
+    return face;
+  }
+
+  freeFace(inFace: ConvexHullBuilderFace): void {
+    // #v-ifdef DEV
+    if (!inFace.removed) {
+      throw new Error("Face should be removed");
+    }
+    // #v-endif
+
+    // Make sure that this face is not connected
+    let e = inFace.firstEdge;
+    if (e !== undefined) {
+      do {
+        // #v-ifdef DEV
+        if (e!.neighbourEdge !== undefined) {
+          throw new Error("Face should not have neighbours");
+        }
+        // #v-endif
+        e = e!.nextEdge;
+      } while (e !== inFace.firstEdge);
+    }
+
+    // Free the face
+    // delete inFace; TODO: may not need to delete anything
+  }
+
+  linkFace(edge1: ConvexHullBuilderEdge, edge2: ConvexHullBuilderEdge) {
+    // Check not connected yet
+    // #v-ifdef DEV
+    if (edge1.neighbourEdge !== undefined) {
+      throw new Error("Edge 1 already connected");
+    }
+    // #v-endif
+
+    // #v-ifdef DEV
+    if (edge2.neighbourEdge !== undefined) {
+      throw new Error("Edge 2 already connected");
+    }
+    // #v-endif
+
+    // #v-ifdef DEV
+    if (edge1.face === edge2.face) {
+      throw new Error("Faces are the same");
+    }
+    // #v-endif
+
+    // // Check not connected yet
+
+    // #v-ifdef DEV
+    // Check faces are different
+    if (edge1.face === edge2.face) {
+      throw new Error("Faces are the same");
+    }
+    // #v-endif
+
+    // #v-ifdef DEV
+    // Check vertices match
+    if (edge1.startIndex !== edge2.nextEdge!.startIndex) {
+      throw new Error("Vertices do not match");
+    }
+    // #v-endif
+
+    // #v-ifdef DEV
+    if (edge2.startIndex !== edge1.nextEdge!.startIndex) {
+      throw new Error("Vertices do not match");
+    }
+    // #v-endif
+
+    // Link up
+    edge1.neighbourEdge = edge2;
+    edge2.neighbourEdge = edge1;
+  }
+
+  unlinkFace(inFace: ConvexHullBuilderFace): void {
+    // Unlink from neighbours
+    let edge = inFace.firstEdge;
+    do {
+      if (edge!.neighbourEdge) {
+        // #v-ifdef DEV
+        // Validate that neighbour points to us
+        if (edge!.neighbourEdge.neighbourEdge !== edge) {
+          throw new Error("Neighbour does not point to us");
+        }
+        // #v-endif
+
+        // Unlink
+        edge!.neighbourEdge.neighbourEdge = undefined;
+        edge!.neighbourEdge = undefined;
+      }
+
+      edge = edge!.nextEdge;
+    } while (edge !== inFace.firstEdge);
+  }
+
+  findEdge(inFacingFace: ConvexHullBuilderFace, inVertex: Vec3, outEdges: ConvexHullBuilderFullEdge[]): void {
+    // #v-ifdef DEV
+    // Assert that we were given an empty array
+    if (outEdges.length !== 0) {
+      throw new Error("outEdges must be empty");
+    }
+    // #v-endif
+
+    // #v-ifdef DEV
+    // Should start with a facing face
+    if (!inFacingFace.isFacing(inVertex)) {
+      throw new Error("inFacingFace must be facing inVertex");
+    }
+    // #v-endif
+
+    // Flag as removed
+    inFacingFace.removed = true;
+
+    // Instead of recursing, we build our own stack with the information we need
+    interface StackEntry {
+      firstEdge: ConvexHullBuilderEdge;
+      currentEdge: ConvexHullBuilderEdge;
+      visited: boolean;
+    }
+
+    const stack: StackEntry[] = [];
+    let currentStackPosition = 0;
+
+    // Start with the face / edge provided
+    stack[currentStackPosition] = {
+      firstEdge: inFacingFace.firstEdge!,
+      currentEdge: inFacingFace.firstEdge!,
+      visited: false,
+    };
+
+    let iteration = -1;
+
+    while (true) {
+      iteration++;
+      const currentEntry = stack[currentStackPosition];
+
+      // Next edge
+      const edge = currentEntry.currentEdge;
+      currentEntry.currentEdge = edge.nextEdge!;
+
+      // If we're back at the first edge we've completed the face and we're done
+      if (edge === currentEntry.firstEdge && currentEntry.visited) {
+        // This face needs to be removed, unlink it now, caller will free
+        this.unlinkFace(edge.face);
+
+        // Pop from stack
+        if (--currentStackPosition < 0) {
+          break;
+        }
+      } else {
+        currentEntry.visited = true;
+        // Visit neighbour face
+        const neighbourEdge = edge.neighbourEdge;
+        if (neighbourEdge) {
+          const neighbourFace = neighbourEdge.face;
+          if (!neighbourFace.removed) {
+            // Check if vertex is on the front side of this face
+            if (neighbourFace.isFacing(inVertex)) {
+              // Vertex on front, this face needs to be removed
+              neighbourFace.removed = true;
+
+              // Add element to the stack of elements to visit
+              currentStackPosition++;
+              stack[currentStackPosition] = {
+                firstEdge: neighbourEdge,
+                currentEdge: neighbourEdge.nextEdge!, // We don't need to test this edge again since we came from it
+                visited: false,
+              };
+            } else {
+              // Vertex behind, keep edge
+              const full = new ConvexHullBuilderFullEdge(edge.startIndex, neighbourEdge.startIndex, neighbourEdge);
+              outEdges.push(full);
+            }
+          }
+        }
+      }
+    }
+  }
+
+  mergeFaces(inEdge: ConvexHullBuilderEdge): void {
+    // Get the face
+    const face = inEdge.face;
+
+    // Find the previous and next edge
+    const nextEdge = inEdge.nextEdge;
+    const prevEdge = inEdge.getPreviousEdge();
+
+    // Get the other face
+    const otherEdge = inEdge.neighbourEdge;
+    const otherFace = otherEdge!.face;
+
+    // #v-ifdef DEV
+    // Check if attempting to merge with self
+    if (face === otherFace) {
+      throw new Error("Attempting to merge with self");
+    }
+    // #v-endif
+
+    // Loop over the edges of the other face and make them belong to inFace
+    let edge = otherEdge!.nextEdge;
+    prevEdge!.nextEdge = edge;
+    while (true) {
+      edge!.face = face;
+      if (edge!.nextEdge === otherEdge) {
+        // Terminate when we are back at otherEdge
+        edge!.nextEdge = nextEdge;
+        break;
+      }
+      edge = edge!.nextEdge;
+    }
+
+    // If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face.
+    // Note that we replace it with the first edge of the merged face so that if the MergeFace function is called
+    // from a loop that loops around the face that it will still terminate after visiting all edges once.
+    if (face.firstEdge === inEdge) {
+      face.firstEdge = prevEdge!.nextEdge;
+    }
+
+    // Free the edges
+    // delete inEdge; TODO: may not need to delete anything
+    // delete otherEdge; TODO: may not need to delete anything
+
+    // Mark the other face as removed
+    otherFace.firstEdge = undefined;
+    otherFace.removed = true;
+
+    // Recalculate plane
+    face.calculateNormalAndCentroid(this.polyhedron!);
+
+    // Merge conflict lists
+    if (face.furthestPointDistanceSquared > otherFace.furthestPointDistanceSquared) {
+      // This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list
+      face.conflictList.splice(face.conflictList.length - 1, 0, ...otherFace.conflictList);
+    } else {
+      // The other face has a point that's furthest away, add that list at the end.
+      face.conflictList.splice(face.conflictList.length, 0, ...otherFace.conflictList);
+      face.furthestPointDistanceSquared = otherFace.furthestPointDistanceSquared;
+    }
+    otherFace.conflictList.length = 0;
+  }
+
+  mergeDegenerateFace(inFace: ConvexHullBuilderFace, ioAffectedFaces: ConvexHullBuilderFace[]): void {
+    mergeDegenerateFacenormal.x = inFace.normal[0];
+    mergeDegenerateFacenormal.y = inFace.normal[1];
+    mergeDegenerateFacenormal.z = inFace.normal[2];
+
+    // Check area of face
+    if (mergeDegenerateFacenormal.squaredLength() < minTriangleAreaSquared) {
+      // Find longest edge, since this face is a sliver this should keep the face convex
+      let maxLengthSquared = 0.0;
+      let longestEdge: ConvexHullBuilderEdge | undefined = undefined;
+      let edge = inFace.firstEdge;
+      mergeDegenerateFacep1.copy(this.polyhedron!.array[edge!.startIndex]);
+      do {
+        const nextEdge = edge!.nextEdge;
+        mergeDegenerateFacep2.copy(this.polyhedron!.array[nextEdge!.startIndex]);
+        const lengthSquared = mergeDegenerateFacep1.squaredDistance(mergeDegenerateFacep2);
+        if (lengthSquared >= maxLengthSquared) {
+          maxLengthSquared = lengthSquared;
+          longestEdge = edge;
+        }
+        mergeDegenerateFacep1.copy(mergeDegenerateFacep2);
+        edge = nextEdge;
+      } while (edge !== inFace.firstEdge);
+
+      // Merge with face on longest edge
+      this.mergeFaces(longestEdge!);
+
+      // Remove any invalid edges
+      this.removeInvalidEdges(inFace, ioAffectedFaces);
+    }
+  }
+
+  mergeCoplanarOrConcaveFaces(
+    inFace: ConvexHullBuilderFace,
+    inCoplanarToleranceSquared: number,
+    ioAffectedFaces: ConvexHullBuilderFace[]
+  ): void {
+    mergeCoplanarOrConcaveFacesinFaceNormal.x = inFace.normal[0];
+    mergeCoplanarOrConcaveFacesinFaceNormal.y = inFace.normal[1];
+    mergeCoplanarOrConcaveFacesinFaceNormal.z = inFace.normal[2];
+    mergeCoplanarOrConcaveFacesinFaceCentroid.x = inFace.centroid[0];
+    mergeCoplanarOrConcaveFacesinFaceCentroid.y = inFace.centroid[1];
+    mergeCoplanarOrConcaveFacesinFaceCentroid.z = inFace.centroid[2];
+
+    let merged = false;
+
+    let edge = inFace.firstEdge;
+    do {
+      // Store next edge since this edge can be removed
+      const nextEdge = edge!.nextEdge;
+
+      // Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq.
+      // If so we need to merge other face into inFace.
+      const otherFace = edge!.neighbourEdge!.face;
+      mergeCoplanarOrConcaveFacesotherFaceNormal.x = otherFace.normal[0];
+      mergeCoplanarOrConcaveFacesotherFaceNormal.y = otherFace.normal[1];
+      mergeCoplanarOrConcaveFacesotherFaceNormal.z = otherFace.normal[2];
+      mergeCoplanarOrConcaveFacesotherFaceCentroid.x = otherFace.centroid[0];
+      mergeCoplanarOrConcaveFacesotherFaceCentroid.y = otherFace.centroid[1];
+      mergeCoplanarOrConcaveFacesotherFaceCentroid.z = otherFace.centroid[2];
+
+      mergeCoplanarOrConcaveFacesdeltaCentroid.subtractVectors(
+        mergeCoplanarOrConcaveFacesotherFaceCentroid,
+        mergeCoplanarOrConcaveFacesinFaceCentroid
+      );
+      const distanceOtherFaceCentroid = mergeCoplanarOrConcaveFacesinFaceNormal.dot(
+        mergeCoplanarOrConcaveFacesdeltaCentroid
+      );
+      const signedDistanceOtherFaceCentroidSquared = Math.abs(distanceOtherFaceCentroid) * distanceOtherFaceCentroid;
+      const distanceFaceCentroid = -mergeCoplanarOrConcaveFacesotherFaceNormal.dot(
+        mergeCoplanarOrConcaveFacesdeltaCentroid
+      );
+      const signedDistanceFaceCentroidSquared = Math.abs(distanceFaceCentroid) * distanceFaceCentroid;
+      const inFaceNormalLengthSquared = mergeCoplanarOrConcaveFacesinFaceNormal.squaredLength();
+      const otherFaceNormalLengthSquared = mergeCoplanarOrConcaveFacesotherFaceNormal.squaredLength();
+      if (
+        (signedDistanceOtherFaceCentroidSquared > -inCoplanarToleranceSquared * inFaceNormalLengthSquared ||
+          signedDistanceFaceCentroidSquared > -inCoplanarToleranceSquared * otherFaceNormalLengthSquared) &&
+        mergeCoplanarOrConcaveFacesinFaceNormal.dot(mergeCoplanarOrConcaveFacesotherFaceNormal) > 0.0
+      ) {
+        // Never merge faces that are back to back
+        this.mergeFaces(edge!);
+        merged = true;
+      }
+
+      edge = nextEdge;
+    } while (edge !== inFace.firstEdge);
+
+    if (merged) {
+      this.removeInvalidEdges(inFace, ioAffectedFaces);
+    }
+  }
+
+  markAffected(inFace: ConvexHullBuilderFace, ioAffectedFaces: ConvexHullBuilderFace[]): void {
+    if (!ioAffectedFaces.includes(inFace)) {
+      ioAffectedFaces.push(inFace);
+    }
+  }
+
+  removeInvalidEdges(inFace: ConvexHullBuilderFace, ioAffectedFaces: ConvexHullBuilderFace[]): void {
+    // This marks that the plane needs to be recalculated (we delay this until the end of the function since we don't use the plane and we want to avoid calculating it multiple times)
+    let recalculatePlane = false;
+
+    // We keep going through this loop until no more edges were removed
+    let removed: boolean;
+
+    do {
+      removed = false;
+
+      // Loop over all edges in this face
+      let edge = inFace.firstEdge;
+      let neighbourFace = edge!.neighbourEdge!.face;
+
+      do {
+        let nextEdge = edge!.nextEdge;
+        let nextNeighbourFace = nextEdge!.neighbourEdge!.face;
+
+        if (neighbourFace === inFace) {
+          // We only remove 1 edge at a time, check if this edge's next edge is our neighbour.
+          // If this check fails, we will continue to scan along the edge until we find an edge where this is the case.
+          if (edge?.neighbourEdge === nextEdge) {
+            // This edge leads back to the starting point, this means the edge is interior and needs to be removed
+
+            // Remove edge
+            const prevEdge = edge!.getPreviousEdge();
+            prevEdge!.nextEdge = nextEdge!.nextEdge;
+            if (inFace.firstEdge === edge || inFace.firstEdge === nextEdge) {
+              inFace.firstEdge = prevEdge;
+            }
+            // delete edge; TODO: may not need to delete anything
+            // delete nextEdge; TODO: may not need to delete anything
+
+            // Check if inFace now has only 2 edges left
+            if (this.removeTwoEdgeFace(inFace, ioAffectedFaces)) {
+              return; // Bail if face no longer exists
+            }
+
+            // Restart the loop
+            recalculatePlane = true;
+            removed = true;
+            break;
+          }
+        } else if (neighbourFace === nextNeighbourFace) {
+          // There are two edges that connect to the same face, we will remove the second one
+
+          // First merge the neighbours edges
+          const neighbourEdge = nextEdge!.neighbourEdge;
+          const nextNeighbourEdge = neighbourEdge!.nextEdge;
+          if (neighbourFace.firstEdge === nextNeighbourEdge) {
+            neighbourFace.firstEdge = neighbourEdge;
+          }
+          neighbourEdge!.nextEdge = nextNeighbourEdge!.nextEdge;
+          neighbourEdge!.neighbourEdge = edge;
+          // delete nextNeighbourEdge; TODO: may not need to delete anything
+
+          // Then merge my own edges
+          if (inFace.firstEdge === nextEdge) {
+            inFace.firstEdge = edge;
+          }
+          edge!.nextEdge = nextEdge!.nextEdge;
+          edge!.neighbourEdge = neighbourEdge;
+          // delete nextEdge; TODO: may not need to delete anything
+
+          // Check if neighbour has only 2 edges left
+          if (!this.removeTwoEdgeFace(neighbourFace, ioAffectedFaces)) {
+            // No, we need to recalculate its plane
+            neighbourFace.calculateNormalAndCentroid(this.polyhedron!);
+
+            // Mark neighbour face as affected
+            this.markAffected(neighbourFace, ioAffectedFaces);
+          }
+
+          // Check if inFace now has only 2 edges left
+          if (this.removeTwoEdgeFace(inFace, ioAffectedFaces)) {
+            return; // Bail if face no longer exists
+          }
+
+          // Restart loop
+          recalculatePlane = true;
+          removed = true;
+          break;
+        }
+
+        // This edge is ok, go to the next edge
+        edge = nextEdge;
+        neighbourFace = nextNeighbourFace;
+      } while (edge !== inFace.firstEdge);
+    } while (removed);
+
+    // Recalculate plane?
+    if (recalculatePlane) {
+      inFace.calculateNormalAndCentroid(this.polyhedron!);
+    }
+  }
+
+  removeTwoEdgeFace(inFace: ConvexHullBuilderFace, ioAffectedFaces: ConvexHullBuilderFace[]): boolean {
+    // Check if this face contains only 2 edges
+    let edge = inFace.firstEdge;
+    let nextEdge = edge!.nextEdge;
+    // #v-ifdef DEV
+    if (edge === nextEdge) {
+      throw new Error("1 edge faces should not exist");
+    }
+    // #v-endif
+
+    if (nextEdge!.nextEdge === edge) {
+      // Schedule both neighbours for re-checking
+      const neighbourEdge = edge!.neighbourEdge;
+      const neighbourFace = neighbourEdge!.face;
+      const nextNeighbourEdge = nextEdge!.neighbourEdge;
+      const nextNeighbourFace = nextNeighbourEdge!.face;
+      this.markAffected(neighbourFace, ioAffectedFaces);
+      this.markAffected(nextNeighbourFace, ioAffectedFaces);
+
+      // Link my neighbours to each other
+      neighbourEdge!.neighbourEdge = nextNeighbourEdge;
+      nextNeighbourEdge!.neighbourEdge = neighbourEdge;
+
+      // Unlink my edges
+      edge!.neighbourEdge = undefined;
+      nextEdge!.neighbourEdge = undefined;
+
+      // Mark this face as removed
+      inFace.removed = true;
+
+      return true;
+    }
+
+    return false;
+  }
+
+  validateFace(inFace: ConvexHullBuilderFace): void {
+    if (inFace.removed) {
+      let edge = inFace.firstEdge;
+      if (edge !== undefined) {
+        do {
+          // #v-ifdef DEV
+          if (edge!.neighbourEdge !== undefined) {
+            throw new Error("Removed face should not have neighbours");
+          }
+          // #v-endif
+          edge = edge!.nextEdge;
+        } while (edge !== inFace.firstEdge);
+      }
+    } else {
+      let edgeCount = 0;
+
+      let edge = inFace.firstEdge;
+      do {
+        // Count edge
+        ++edgeCount;
+
+        // Validate that adjacent faces are all different
+        if (this.faces.length > 2) {
+          for (let otherEdge = edge!.nextEdge; otherEdge !== inFace.firstEdge; otherEdge = otherEdge!.nextEdge) {
+            // #v-ifdef DEV
+            if (edge!.neighbourEdge!.face === otherEdge!.neighbourEdge!.face) {
+              throw new Error("Adjacent faces should be different");
+            }
+            // #v-endif
+          }
+
+          // #v-ifdef DEV
+          // Assert that the face is correct
+          if (edge!.face !== inFace) {
+            throw new Error("Face is not correct");
+          }
+          // #v-endif
+
+          const nbEdge = edge!.neighbourEdge;
+          // #v-ifdef DEV
+          // Assert that we have a neighbour
+          if (nbEdge === undefined) {
+            throw new Error("Edge should have a neighbour");
+          }
+          // #v-endif
+
+          // #v-ifdef DEV
+          // Assert that our neighbours edge points to us
+          if (nbEdge.neighbourEdge !== edge) {
+            throw new Error("Neighbour should point to us");
+          }
+          // #v-endif
+
+          // #v-ifdef DEV
+          // Assert that it belongs to a different face
+          if (nbEdge.face === inFace) {
+            throw new Error("Neighbour should belong to a different face");
+          }
+          // #v-endif
+
+          // #v-ifdef DEV
+          // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
+          if (nbEdge.nextEdge!.startIndex !== edge!.startIndex) {
+            throw new Error("Neighbour's next edge should point to the same vertex");
+          }
+
+          // #v-endif
+          // #v-ifdef DEV
+          // Assert that my next edge points to the same vertex as my neighbours vertex
+          if (edge!.nextEdge!.startIndex !== nbEdge.startIndex) {
+            throw new Error("My next edge should point to the same vertex as my neighbours vertex");
+          }
+          // #v-endif
+        }
+
+        edge = edge!.nextEdge;
+      } while (edge !== inFace.firstEdge);
+
+      // #v-ifdef DEV
+      // Assert that we have 3 or more edges
+      if (edgeCount < 3) {
+        throw new Error("Face should have 3 or more edges");
+      }
+      // #v-endif
+    }
+  }
+
+  validateFaces(): void {
+    for (const face of this.faces) {
+      this.validateFace(face);
+    }
+  }
+
+  getCenterOfMassAndVolume(outCenterOfMass: Vec3): number {
+    // Fourth point is the average of all face centroids
+    getCenterOfMassAndVolumevertexD.zero();
+    for (const face of this.faces) {
+      getCenterOfMassAndVolumevertexD.x += face.centroid[0];
+      getCenterOfMassAndVolumevertexD.y += face.centroid[1];
+      getCenterOfMassAndVolumevertexD.z += face.centroid[2];
+    }
+
+    getCenterOfMassAndVolumevertexD.scaleVector(getCenterOfMassAndVolumevertexD, 1.0 / this.faces.length);
+
+    // Calculate mass and center of mass of this convex hull by summing all tetrahedrons
+    let outVolume = 0.0;
+    outCenterOfMass.zero();
+    for (const face of this.faces) {
+      // Get the first vertex that we'll use to create a triangle fan
+      let edge = face.firstEdge;
+      getCenterOfMassAndVolumevertexA.copy(this.polyhedron!.array[edge!.startIndex]);
+
+      // Get the second vertex
+      edge = edge!.nextEdge;
+      getCenterOfMassAndVolumevertexB.copy(this.polyhedron!.array[edge!.startIndex]);
+
+      for (edge = edge!.nextEdge; edge !== face.firstEdge; edge = edge!.nextEdge) {
+        // Fetch the last point of the triangle
+        getCenterOfMassAndVolumevertexC.copy(this.polyhedron!.array[edge!.startIndex]);
+
+        // Calculate center of mass and mass of this tetrahedron,
+        // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
+        getCenterOfMassAndVolumevectorDA.subtractVectors(
+          getCenterOfMassAndVolumevertexA,
+          getCenterOfMassAndVolumevertexD
+        );
+        getCenterOfMassAndVolumevectorDB.subtractVectors(
+          getCenterOfMassAndVolumevertexB,
+          getCenterOfMassAndVolumevertexD
+        );
+        getCenterOfMassAndVolumevectorDC.subtractVectors(
+          getCenterOfMassAndVolumevertexC,
+          getCenterOfMassAndVolumevertexD
+        );
+        getCenterOfMassAndVolumecrossDB_DC.crossVectors(
+          getCenterOfMassAndVolumevectorDB,
+          getCenterOfMassAndVolumevectorDC
+        );
+        const volume_tetrahedron = getCenterOfMassAndVolumevectorDA.dot(getCenterOfMassAndVolumecrossDB_DC); // Needs to be divided by 6, postpone this until the end of the loop
+        getCenterOfMassAndVolumecenterOfMassTetrahedron.addVectors(
+          getCenterOfMassAndVolumevertexA,
+          getCenterOfMassAndVolumevertexB
+        );
+        getCenterOfMassAndVolumecenterOfMassTetrahedron.addVectors(
+          getCenterOfMassAndVolumecenterOfMassTetrahedron,
+          getCenterOfMassAndVolumevertexC
+        );
+        getCenterOfMassAndVolumecenterOfMassTetrahedron.addVectors(
+          getCenterOfMassAndVolumecenterOfMassTetrahedron,
+          getCenterOfMassAndVolumevertexD
+        ); // Needs to be divided by 4, postpone this until the end of the loop
+
+        // Accumulate results
+        outVolume += volume_tetrahedron;
+        outCenterOfMass.addScaledToVector(
+          outCenterOfMass,
+          getCenterOfMassAndVolumecenterOfMassTetrahedron,
+          volume_tetrahedron
+        );
+
+        // Update v2 for next triangle
+        getCenterOfMassAndVolumevertexB.copy(getCenterOfMassAndVolumevertexC);
+      }
+    }
+
+    // Calculate center of mass, fall back to average point in case there is no volume (everything is on a plane in this case)
+    if (outVolume > floatEpsilon) {
+      outCenterOfMass.scaleVector(outCenterOfMass, 1.0 / (4.0 * outVolume));
+    } else {
+      outCenterOfMass.copy(getCenterOfMassAndVolumevertexD);
+    }
+
+    outVolume /= 6.0;
+
+    return outVolume;
+  }
+
+  determineMaxError(): [ConvexHullBuilderFace | undefined, number, number, number] {
+    const outCoplanarDistance = this.determineCoplanarDistance();
+
+    // This measures the distance from a polygon to the furthest point outside of the hull
+    let maxError = 0.0;
+    let maxErrorFace: ConvexHullBuilderFace | undefined = undefined;
+    let maxErrorPoint = -1;
+
+    for (let i = 0; i < this.polyhedron!.length; ++i) {
+      determineMaxError.copy(this.polyhedron!.array[i]);
+
+      // This measures the closest edge from all faces to point v
+      // Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face
+      // we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar
+      // polygon.
+      let minEdgeDistanceSquared = Infinity;
+      let minEdgeDistanceFace: ConvexHullBuilderFace | undefined = undefined;
+
+      for (const face of this.faces) {
+        // Check if point is on or in front of plane
+        determineMaxErrornormal.x = face.normal[0];
+        determineMaxErrornormal.y = face.normal[1];
+        determineMaxErrornormal.z = face.normal[2];
+        determineMaxErrorcentroid.x = face.centroid[0];
+        determineMaxErrorcentroid.y = face.centroid[1];
+        determineMaxErrorcentroid.z = face.centroid[2];
+        const normalLength = determineMaxErrornormal.length();
+        // #v-ifdef DEV
+        if (normalLength <= 0.0) {
+          throw new Error(`Normal length should be greater than 0, got ${normalLength}`);
+        }
+        // #v-endif
+        determineMaxErrortempVector.subtractVectors(determineMaxError, determineMaxErrorcentroid);
+        const planeDistance = determineMaxError.dot(determineMaxErrornormal) / normalLength;
+        if (planeDistance > -outCoplanarDistance) {
+          // Check distance to the edges of this face
+          const edgeDistanceSquared = this.getDistanceToEdgeSquared(determineMaxError, face);
+          if (edgeDistanceSquared < minEdgeDistanceSquared) {
+            minEdgeDistanceSquared = edgeDistanceSquared;
+            minEdgeDistanceFace = face;
+          }
+
+          // If the point is inside the polygon and the point is in front of the plane, measure the distance
+          if (edgeDistanceSquared === 0.0 && planeDistance > maxError) {
+            maxError = planeDistance;
+            maxErrorFace = face;
+            maxErrorPoint = i;
+          }
+        }
+      }
+
+      // If the minimum distance to an edge is further than our current max error, we use that as max error
+      const minEdgeDistance = Math.sqrt(minEdgeDistanceSquared);
+      if (minEdgeDistanceFace !== undefined && minEdgeDistance > maxError) {
+        maxError = minEdgeDistance;
+        maxErrorFace = minEdgeDistanceFace;
+        maxErrorPoint = i;
+      }
+    }
+
+    return [maxErrorFace, maxError, maxErrorPoint, outCoplanarDistance];
+  }
+}