@perplexdotgg/bounce 1.0.0 → 1.0.1

package/src/collision/gjk/GjkModule.ts

@@ -1,864 +0,0 @@
-import { BooleanType, createClass, MonomorphType, NumberType, PropertyDefinitionMap } from "monomorph";
-import { Vec3 } from "../../math/vec3";
-import { computeBarycentricCoordinates2d } from "../closestPoints/computeBarycentricCoordinates2d";
-import { computeBarycentricCoordinates3d } from "../closestPoints/computeBarycentricCoordinates3d";
-import { BarycentricCoordinatesResult, ClosestPointResult } from "../closestPoints/closestPoints";
-import { isClose, squared } from "../../math/scalar";
-import { computeClosestPointOnLine } from "../closestPoints/computeClosestPointOnLine";
-import { computeClosestPointOnTriangle } from "../closestPoints/computeClosestPointOnTriangle";
-import { computeClosestPointOnTetrahedron } from "../closestPoints/computeClosestPointOnTetrahedron";
-import { Isometry } from "../../math/isometry";
-import { SupportShape, TransformedConvexObject } from "../../shape/Convex";
-import { SupportPoints } from "./SupportPoints";
-import { Ray } from "../../shape/Ray";
-
-const closestPointToSimplexProps = {
-  point: MonomorphType(Vec3),
-  squaredDistance: NumberType(0),
-  pointSet: NumberType(0),
-  closestPointFound: BooleanType(false),
-} as const satisfies PropertyDefinitionMap;
-
-export class ClosestPointToSimplex extends createClass<ClosestPointToSimplex, typeof closestPointToSimplexProps>(
-  closestPointToSimplexProps
-) {}
-
-const gjkClosestPointsProps = {
-  squaredDistance: NumberType(0),
-  penetrationAxis: MonomorphType(Vec3),
-  pointA: MonomorphType(Vec3),
-  pointB: MonomorphType(Vec3),
-} as const satisfies PropertyDefinitionMap;
-
-export class GjkClosestPoints extends createClass<GjkClosestPoints, typeof gjkClosestPointsProps>(
-  gjkClosestPointsProps
-) {
-  reset(): void {
-    this.pointA.zero();
-    this.pointB.zero();
-    this.penetrationAxis.zero();
-    this.squaredDistance = 0;
-  }
-}
-
-const gjkCastShapeResultProps = {
-  isHitFound: BooleanType(false),
-  lambda: NumberType(0),
-  separatingAxis: MonomorphType(Vec3),
-  pointA: MonomorphType(Vec3),
-  pointB: MonomorphType(Vec3),
-} as const satisfies PropertyDefinitionMap;
-
-export class GjkCastShapeResult extends createClass<GjkCastShapeResult, typeof gjkCastShapeResultProps>(
-  gjkCastShapeResultProps
-) {
-  reset(): void {
-    this.isHitFound = false;
-    this.pointA.zero();
-    this.pointB.zero();
-    this.separatingAxis.zero();
-    this.lambda = 1;
-  }
-}
-
-type Simplex = [Vec3, Vec3, Vec3, Vec3];
-
-const transformedConvexObject = /*@__PURE__*/ new TransformedConvexObject();
-
-const y0 = /*@__PURE__*/ Vec3.create();
-const y1 = /*@__PURE__*/ Vec3.create();
-const y2 = /*@__PURE__*/ Vec3.create();
-const y3 = /*@__PURE__*/ Vec3.create();
-const ys: Simplex = [y0, y1, y2, y3];
-
-const p0 = /*@__PURE__*/ Vec3.create();
-const p1 = /*@__PURE__*/ Vec3.create();
-const p2 = /*@__PURE__*/ Vec3.create();
-const p3 = /*@__PURE__*/ Vec3.create();
-const ps: Simplex = [p0, p1, p2, p3];
-
-const q0 = /*@__PURE__*/ Vec3.create();
-const q1 = /*@__PURE__*/ Vec3.create();
-const q2 = /*@__PURE__*/ Vec3.create();
-const q3 = /*@__PURE__*/ Vec3.create();
-const qs: Simplex = [q0, q1, q2, q3];
-
-const bary = /*@__PURE__*/ BarycentricCoordinatesResult.create();
-
-const closest = /*@__PURE__*/ ClosestPointResult.create();
-
-const p = /*@__PURE__*/ Vec3.create();
-const q = /*@__PURE__*/ Vec3.create();
-const w = /*@__PURE__*/ Vec3.create();
-const directionA = /*@__PURE__*/ Vec3.create();
-const directionB = /*@__PURE__*/ Vec3.create();
-const closestPointToSimplex = /*@__PURE__*/ ClosestPointToSimplex.create();
-
-const directionA_2 = /*@__PURE__*/ Vec3.create();
-const directionB_2 = /*@__PURE__*/ Vec3.create();
-const x = /*@__PURE__*/ Vec3.create();
-const prev_v = /*@__PURE__*/ Vec3.create();
-const pq = /*@__PURE__*/ Vec3.create();
-const normalized_v = /*@__PURE__*/ Vec3.create();
-const convex_radius_a = /*@__PURE__*/ Vec3.create();
-const convex_radius_b = /*@__PURE__*/ Vec3.create();
-const zero = /*@__PURE__*/ Vec3.create();
-
-// Calculate closest points on A and B
-function computePointsAAndB(outPointA: Vec3, outPointB: Vec3, numPoints: number): void {
-  outPointA.zero();
-  outPointB.zero();
-
-  switch (numPoints) {
-    case 1: {
-      outPointA.copy(ps[0]);
-      outPointB.copy(qs[0]);
-      break;
-    }
-
-    case 2: {
-      computeBarycentricCoordinates2d(bary, ys[0], ys[1]);
-
-      outPointA.addScaled(ps[0], bary.u);
-      outPointA.addScaled(ps[1], bary.v);
-
-      outPointB.addScaled(qs[0], bary.u);
-      outPointB.addScaled(qs[1], bary.v);
-      break;
-    }
-
-    case 3: {
-      computeBarycentricCoordinates3d(bary, ys[0], ys[1], ys[2]);
-
-      outPointA.addScaled(ps[0], bary.u);
-      outPointA.addScaled(ps[1], bary.v);
-      outPointA.addScaled(ps[2], bary.w);
-
-      outPointB.addScaled(qs[0], bary.u);
-      outPointB.addScaled(qs[1], bary.v);
-      outPointB.addScaled(qs[2], bary.w);
-      break;
-    }
-
-    default: {
-      break;
-    }
-  }
-}
-
-function getMaxYLengthSquared(numPoints: number): number {
-  let yLengthSquared = ys[0].squaredLength();
-  for (let i = 1; i < numPoints; ++i) {
-    yLengthSquared = Math.max(yLengthSquared, ys[i].squaredLength());
-  }
-  return yLengthSquared;
-}
-
-// Remove points that are not in the set, only updates mP
-function updatePointSetP(inSet: number, numPoints: number): number {
-  let newNumPoints = 0;
-  for (let i = 0; i < numPoints; ++i) {
-    if ((inSet & (1 << i)) !== 0) {
-      ps[newNumPoints].copy(ps[i]);
-      ++newNumPoints;
-    }
-  }
-  return newNumPoints;
-}
-
-// Remove points that are not in the set, only updates mP and mQ
-function updatePointSetPQ(inSet: number, numPoints: number): number {
-  let newNumPoints = 0;
-  for (let i = 0; i < numPoints; ++i) {
-    if ((inSet & (1 << i)) !== 0) {
-      ps[newNumPoints].copy(ps[i]);
-      qs[newNumPoints].copy(qs[i]);
-      ++newNumPoints;
-    }
-  }
-  return newNumPoints;
-}
-
-// Remove points that are not in the set, updates y, p, q
-function updatePointSetYPQ(inSet: number, numPoints: number): number {
-  let newNumPoints = 0;
-  for (let i = 0; i < numPoints; ++i) {
-    if ((inSet & (1 << i)) !== 0) {
-      ys[newNumPoints].copy(ys[i]);
-      ps[newNumPoints].copy(ps[i]);
-      qs[newNumPoints].copy(qs[i]);
-      ++newNumPoints;
-    }
-  }
-  return newNumPoints;
-}
-
-const vvv = /*@__PURE__*/ Vec3.create();
-
-function getClosest(
-  inPrevVLenSq: number,
-  outV: Vec3,
-  outVLenSq: { value: number },
-  outSet: { value: number },
-  lastPointPartOfClosestFeature: boolean,
-  numPoints: number,
-  simplex: Simplex
-): boolean {
-  let set = 0;
-  vvv.zero();
-
-  switch (numPoints) {
-    // Single point
-    case 1: {
-      set = 0b0001;
-      vvv.copy(ys[0]);
-      break;
-    }
-
-    // Line segment
-    case 2: {
-      computeClosestPointOnLine(closest, simplex[0], simplex[1]);
-      set = closest.pointSet;
-      vvv.copy(closest.point);
-      break;
-    }
-    // Triangle
-    case 3: {
-      computeClosestPointOnTriangle(closest, simplex[0], simplex[1], simplex[2], lastPointPartOfClosestFeature);
-      set = closest.pointSet;
-      vvv.copy(closest.point);
-      break;
-    }
-
-    // Tetrahedron
-    case 4: {
-      computeClosestPointOnTetrahedron(
-        closest,
-        simplex[0],
-        simplex[1],
-        simplex[2],
-        simplex[3],
-        lastPointPartOfClosestFeature
-      );
-      set = closest.pointSet;
-      vvv.copy(closest.point);
-      break;
-    }
-    default: {
-      throw new Error("Invalid number of points in simplex");
-    }
-  }
-
-  let v_len_sq = vvv.squaredLength();
-  // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
-  if (v_len_sq < inPrevVLenSq) {
-    // Return closest point
-    outV.copy(vvv);
-    outVLenSq.value = v_len_sq;
-    outSet.value = set;
-    return true;
-  }
-
-  // No better match found
-  return false;
-}
-
-function computeClosestPointToSimplex(
-  result: ClosestPointToSimplex,
-  inPreviousSquaredDistance: number,
-  lastPointPartOfClosestFeature: boolean,
-  simplex: Simplex,
-  numPoints: number
-): void {
-  switch (numPoints) {
-    case 1: {
-      // Single point
-      closest.pointSet = 0b0001;
-      closest.point.copy(simplex[0]);
-      break;
-    }
-
-    case 2: {
-      // Line segment
-      computeClosestPointOnLine(closest, simplex[0], simplex[1]);
-      break;
-    }
-
-    case 3: {
-      // Triangle
-      computeClosestPointOnTriangle(closest, simplex[0], simplex[1], simplex[2], lastPointPartOfClosestFeature);
-      break;
-    }
-
-    case 4: {
-      // Tetrahedron
-      computeClosestPointOnTetrahedron(
-        closest,
-        simplex[0],
-        simplex[1],
-        simplex[2],
-        simplex[3],
-        lastPointPartOfClosestFeature
-      );
-      break;
-    }
-
-    default: {
-      throw new Error("Invalid number of points");
-    }
-  }
-
-  const squaredDistance = closest.point.squaredLength();
-  if (squaredDistance < inPreviousSquaredDistance) {
-    // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
-
-    // Return closest point
-    result.point.copy(closest.point);
-    result.squaredDistance = squaredDistance;
-    result.pointSet = closest.pointSet;
-    result.closestPointFound = true;
-    return;
-  }
-
-  // No better match found
-  // logDebug("New closer point is further away, failed to converge");
-  result.closestPointFound = false;
-}
-
-export interface GjkModuleSettings {
-  maxIterations: number;
-  tolerance: number;
-}
-
-export class GjkModule {
-  numPoints: number;
-  settings: GjkModuleSettings;
-  maxIterations: number;
-  tolerance: number;
-
-  constructor(settings: Partial<GjkModuleSettings> = {}) {
-    this.numPoints = 0;
-    this.settings = {
-      ...GjkModule.createDefaultSettings(),
-      ...settings,
-    };
-    this.maxIterations = this.settings.maxIterations;
-    this.tolerance = this.settings.tolerance;
-  }
-
-  static createDefaultSettings(): GjkModuleSettings {
-    return {
-      maxIterations: 100,
-      tolerance: 1e-5,
-    };
-  }
-
-  getClosestPointsSimplex(supportPoints: SupportPoints) {
-    for (let i = 0; i < this.numPoints; i++) {
-      supportPoints.mY.pushBackCopy(ys[i]);
-      supportPoints.mP.pushBackCopy(ps[i]);
-      supportPoints.mQ.pushBackCopy(qs[i]);
-    }
-  }
-
-  getClosestPoints(
-    result: GjkClosestPoints,
-    inA: SupportShape,
-    inB: SupportShape,
-    inTolerance: number,
-    inMaxDistanceSquared: number,
-    inDirection: Vec3
-  ): void {
-    const squaredTolerance = squared(inTolerance);
-
-    // Reset state
-    this.numPoints = 0;
-
-    // Length^2 of v
-    closestPointToSimplex.squaredDistance = inDirection.squaredLength();
-    closestPointToSimplex.point.copy(inDirection);
-
-    // Previous length^2 of v
-    // TODO: there was a bug here caused by using Infinity instead of Number.MAX_VALUE due to the value being used in a comparison, is this or similar happening anywhere else?
-    // let previousSquaredDistance = Infinity;
-    let previousSquaredDistance = Number.MAX_VALUE;
-
-    let iterations = 0;
-    while (iterations++ < this.maxIterations) {
-      // #v-ifdef DEV
-      if (this.numPoints >= 4) {
-        throw new Error("numPoints >= 4");
-      }
-      // #v-endif
-
-      // Get support points for shape A and B in the direction of v
-      directionA.copy(closestPointToSimplex.point);
-      directionB.negateVector(closestPointToSimplex.point);
-      inA.computeSupport(p, directionA);
-      inB.computeSupport(q, directionB);
-
-      // Get support point of the minkowski sum A - B of v
-      w.subtractVectors(p, q);
-
-      const dot = closestPointToSimplex.point.dot(w);
-
-      // Test if we have a separation of more than inMaxDistSq, in which case we terminate early
-      if (dot < 0.0 && dot * dot > closestPointToSimplex.squaredDistance * inMaxDistanceSquared) {
-        result.squaredDistance = Infinity;
-        return;
-      }
-
-      // Store the point for later use
-      ys[this.numPoints].copy(w);
-      ps[this.numPoints].copy(p);
-      qs[this.numPoints].copy(q);
-      ++this.numPoints;
-
-      closestPointToSimplex.pointSet = 0;
-      computeClosestPointToSimplex(closestPointToSimplex, previousSquaredDistance, true, ys, this.numPoints);
-      if (!closestPointToSimplex.closestPointFound) {
-        // Undo add last point
-        --this.numPoints;
-        break;
-      }
-
-      // If there are 4 points, the origin is inside the tetrahedron and we're done
-      if (closestPointToSimplex.pointSet === 0xf) {
-        // logDebug("Full simplex");
-        closestPointToSimplex.point.zero();
-        closestPointToSimplex.squaredDistance = 0.0;
-        break;
-      }
-
-      // Update the points of the simplex
-      this.numPoints = updatePointSetYPQ(closestPointToSimplex.pointSet, this.numPoints);
-
-      // If v  is very close to zero, we consider this a collision
-      if (closestPointToSimplex.squaredDistance <= squaredTolerance) {
-        // logDebug("Distance zero");
-        closestPointToSimplex.point.zero();
-        closestPointToSimplex.squaredDistance = 0.0;
-        break;
-      }
-
-      // If v is very small compared to the length of y, we also consider this a collision
-      if (closestPointToSimplex.squaredDistance <= this.tolerance * getMaxYLengthSquared(this.numPoints)) {
-        // logDebug("Machine precision reached");
-        closestPointToSimplex.point.zero();
-        closestPointToSimplex.squaredDistance = 0.0;
-        break;
-      }
-
-      // The next seperation axis to test is the negative of the closest point of the Minkowski sum to the origin
-      // Note: This must be done before terminating as converged since the separating axis is -v
-      closestPointToSimplex.point.negate();
-
-      // #v-ifdef DEV
-      // If the squared length of v is not changing enough, we've converged and there is no collision
-      if (previousSquaredDistance < closestPointToSimplex.squaredDistance) {
-        throw new Error("squared distance not decreasing");
-      }
-      // #v-endif
-
-      if (previousSquaredDistance - closestPointToSimplex.squaredDistance <= this.tolerance * previousSquaredDistance) {
-        // v is a separating axis
-        // logDebug("Converged");
-        break;
-      }
-
-      previousSquaredDistance = closestPointToSimplex.squaredDistance;
-    }
-
-    // Get the closest points
-    computePointsAAndB(result.pointA, result.pointB, this.numPoints);
-
-    // #v-ifdef DEV
-    // assert ioV.LengthSq() == v_len_sq;
-    if (!isClose(closestPointToSimplex.point.squaredLength(), closestPointToSimplex.squaredDistance, 1e-3)) {
-      throw new Error("closest point squared length is not equal to closest squared distance");
-    }
-    // #v-endif
-
-    result.squaredDistance = closestPointToSimplex.squaredDistance;
-    result.penetrationAxis.copy(closestPointToSimplex.point); //ioV
-  }
-
-  castShape(
-    result: GjkCastShapeResult, // contains return_value, ioLambda (lambda), outPointA (pointA), outPointB (pointB), outSeparatingAxis (separatingAxis)
-    inStart: Isometry,
-    inDirection: Vec3,
-    inTolerance: number,
-    inA: SupportShape,
-    inB: SupportShape,
-    inConvexRadiusA: number,
-    inConvexRadiusB: number
-  ): void {
-    zero.zero();
-
-    let squaredTolerance = squared(inTolerance);
-
-    // Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision
-    const sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
-
-    // Transform the shape to be cast to the starting position
-    const transformed_a = transformedConvexObject;
-    transformed_a.setData(inStart, inA);
-
-    // Reset state
-    this.numPoints = 0;
-
-    let lambda = 0.0;
-
-    // Since A is already transformed we can start the cast from zero
-    x.zero();
-
-    directionB_2.zero();
-    inB.computeSupport(q, directionB_2);
-    q.negate();
-
-    directionA_2.zero();
-    transformed_a.computeSupport(p, directionA_2);
-
-    // SOURCE
-    // vec3.add(closest.point, q, p);
-    // CHANGE
-    closestPointToSimplex.point.subtractVectors(q, p);
-
-    // let v_len_sq = Number.MAX_VALUE;
-    closestPointToSimplex.squaredDistance = Number.MAX_VALUE;
-    let allow_restart = false;
-
-    // Keeps track of separating axis of the previous iteration.
-    // Initialized at zero as we don't know if our first v is actually a separating axis.
-    prev_v.zero();
-
-    let iterations = 0;
-    while (iterations++ < this.maxIterations) {
-      // Calculate the minkowski difference inB - inA
-      // inA is moving, so we need to add the back side of inB to the front side of inA
-      // Keep the support points on A and B separate so that in the end we can calculate a contact point
-      directionA_2.negateVector(closestPointToSimplex.point);
-      transformed_a.computeSupport(p, directionA_2);
-
-      directionB_2.copy(closestPointToSimplex.point);
-      inB.computeSupport(q, directionB_2);
-
-      pq.subtractVectors(q, p);
-      w.subtractVectors(x, pq);
-
-      // Difference from article to this code:
-      // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm.
-      // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii.
-      // From p we have to subtract: inConvexRadiusA * v / |v|
-      // To q we have to add: inConvexRadiusB * v / |v|
-      // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v|
-      // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v|
-      const v_dot_w = closestPointToSimplex.point.dot(w) - sum_convex_radius * closestPointToSimplex.point.length();
-      if (v_dot_w > 0.0) {
-        // If ray and normal are in the same direction, we've passed A and there's no collision
-        const v_dot_r = closestPointToSimplex.point.dot(inDirection);
-        if (v_dot_r >= 0.0) {
-          result.isHitFound = false;
-          return;
-        }
-
-        // Update the lower bound for lambda
-        const delta = v_dot_w / v_dot_r;
-        const old_lambda = lambda;
-        lambda -= delta;
-
-        // If lambda didn't change, we cannot converge any further and we assume a hit
-        if (old_lambda === lambda) {
-          break;
-        }
-
-        // If lambda is bigger or equal than max, we don't have a hit
-        if (lambda >= result.lambda) {
-          result.isHitFound = false;
-          return;
-        }
-
-        // Update x to new closest point on the ray
-        x.scaleVector(inDirection, lambda);
-
-        // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false
-        closestPointToSimplex.squaredDistance = Number.MAX_VALUE;
-
-        // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating
-        // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart
-        squaredTolerance = squared(inTolerance + sum_convex_radius);
-
-        // We allow rebuilding the simplex once after x changes because the simplex was built
-        // for another x and numerical round off builds up as you keep adding points to an
-        // existing simplex
-        allow_restart = true;
-      }
-
-      // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q}
-      ps[this.numPoints].copy(p);
-      qs[this.numPoints].copy(q);
-      ++this.numPoints;
-
-      // Calculate Y = {x} - (Q - P)
-      for (let i = 0; i < this.numPoints; ++i) {
-        pq.subtractVectors(qs[i], ps[i]);
-        ys[i].subtractVectors(x, pq);
-      }
-
-      // Determine the new closest point from Y to origin
-      closestPointToSimplex.pointSet = 0;
-      computeClosestPointToSimplex(
-        closestPointToSimplex,
-        closestPointToSimplex.squaredDistance,
-        false,
-        ys,
-        this.numPoints
-      );
-      if (!closestPointToSimplex.closestPointFound) {
-        // Only allow 1 restart, if we still can't get a closest point
-        // we're so close that we return this as a hit
-        if (!allow_restart) {
-          // logDebug("Failed to converge");
-          break;
-        }
-
-        // If we fail to converge, we start again with the last point as simplex
-        // logDebug("Restarting");
-
-        allow_restart = false;
-        ps[0].copy(p);
-        this.numPoints = 1;
-        closestPointToSimplex.point.subtractVectors(x, p);
-        closestPointToSimplex.squaredDistance = Number.MAX_VALUE;
-        continue;
-      } else if (closestPointToSimplex.pointSet === 0xf) {
-        // logDebug("Full simplex");
-
-        // #v-ifdef DEV
-        // We're inside the tetrahedron, we have a hit (verify that length of v is 0)
-        if (closestPointToSimplex.squaredDistance !== 0.0) {
-          throw new Error("v length must be 0");
-        }
-        // #v-endif
-      }
-
-      // Update the points P and Q to form the new simplex
-      // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
-      this.numPoints = updatePointSetPQ(closestPointToSimplex.pointSet, this.numPoints);
-
-      // Check if A and B are touching according to our tolerance
-      if (closestPointToSimplex.squaredDistance <= squaredTolerance) {
-        // logDebug("Converged");
-        break;
-      }
-
-      // Store our v to return as separating axis
-      prev_v.copy(closestPointToSimplex.point);
-    }
-
-    // Calculate Y = {x} - (Q - P) again so we can calculate the contact points
-    for (let i = 0; i < this.numPoints; ++i) {
-      pq.subtractVectors(qs[i], ps[i]);
-      ys[i].subtractVectors(x, pq);
-    }
-
-    // Calculate the offset we need to apply to A and B to correct for the convex radius
-    closestPointToSimplex.point.normalizedOr(normalized_v, zero);
-    convex_radius_a.scaleVector(normalized_v, inConvexRadiusA);
-    convex_radius_b.scaleVector(normalized_v, inConvexRadiusB);
-
-    const bary = /*@__PURE__*/ BarycentricCoordinatesResult.create();
-    result.pointA.zero();
-    result.pointB.zero();
-
-    // Get the contact point
-    // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms.
-    switch (this.numPoints) {
-      case 1: {
-        result.pointB.addVectors(qs[0], convex_radius_b);
-        if (lambda > 0.0) {
-          result.pointA.copy(result.pointB);
-        } else {
-          result.pointA.subtractVectors(ps[0], convex_radius_a);
-        }
-        break;
-      }
-      case 2: {
-        computeBarycentricCoordinates2d(bary, ys[0], ys[1]);
-
-        result.pointB.addScaled(qs[0], bary.u);
-        result.pointB.addScaled(qs[1], bary.v);
-        result.pointB.addVectors(result.pointB, convex_radius_b);
-
-        if (lambda > 0.0) {
-          result.pointA.copy(result.pointB);
-        } else {
-          result.pointA.addScaled(ps[0], bary.u);
-          result.pointA.addScaled(ps[1], bary.v);
-          result.pointA.subtractVectors(result.pointA, convex_radius_a);
-        }
-        break;
-      }
-      case 3:
-      case 4: {
-        // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration.
-        computeBarycentricCoordinates3d(bary, ys[0], ys[1], ys[2]);
-
-        result.pointB.addScaled(qs[0], bary.u);
-        result.pointB.addScaled(qs[1], bary.v);
-        result.pointB.addScaled(qs[2], bary.w);
-        result.pointB.addVectors(result.pointB, convex_radius_b);
-
-        if (lambda > 0.0) {
-          result.pointA.copy(result.pointB);
-        } else {
-          result.pointA.addScaled(ps[0], bary.u);
-          result.pointA.addScaled(ps[1], bary.v);
-          result.pointA.addScaled(ps[2], bary.w);
-          result.pointA.subtractVectors(result.pointA, convex_radius_a);
-        }
-        break;
-      }
-      default: {
-        throw new Error("Invalid number of points");
-      }
-    }
-
-    // Store separating axis, in case we have a convex radius we can just return v,
-    // otherwise v will be very small and we resort to returning previous v as an approximation.
-    if (sum_convex_radius > 0.0) {
-      result.separatingAxis.negateVector(closestPointToSimplex.point);
-    } else {
-      result.separatingAxis.negateVector(prev_v);
-    }
-
-    // Store hit fraction
-    result.lambda = lambda;
-    result.isHitFound = true;
-  }
-
-  castRay(
-    inRayOrigin: Vec3,
-    inRayDirection: Vec3,
-    inTolerance: number,
-    inA: SupportShape,
-    ioLambda: { fraction: number }
-  ): boolean {
-    const squaredTolerance = squared(inTolerance);
-
-    // Reset state
-    this.numPoints = 0;
-
-    let lambda = 0.0;
-    x.copy(inRayOrigin);
-    directionB_2.zero();
-    inA.computeSupport(p, directionB_2);
-    v.subtractVectors(x, p);
-    let v_len_sq = { value: Number.MAX_VALUE };
-    let allowRestart = false;
-
-    let iterations = 0;
-    while (true) {
-      if (iterations >= this.maxIterations) {
-        return false;
-      }
-
-      // Get new support point
-      inA.computeSupport(p, v);
-      w.subtractVectors(x, p);
-
-      const vDotW = v.dot(w);
-
-      if (vDotW > 0.0) {
-        // If ray and normal are in the same direction, we've passed A and there's no collision
-        const vDotR = v.dot(inRayDirection);
-
-        // Instead of checking >= 0, check with epsilon as we don't want the division below to overflow to infinity as it can cause a float exception
-        if (vDotR >= -1.0e-18) {
-          return false;
-        }
-
-        // Update the lower bound for lambda
-        const delta = vDotW / vDotR;
-        const oldLambda = lambda;
-        lambda -= delta;
-
-        // If lambda didn't change, we cannot converge any further and we assume a hit
-        if (oldLambda === lambda) {
-          break;
-        }
-
-        // If lambda is bigger or equal than max, we don't have a hit
-        if (lambda >= ioLambda.fraction) {
-          return false;
-        }
-
-        // Update x to new closest point on the ray
-        x.addScaledToVector(inRayOrigin, inRayDirection, lambda);
-
-        // We've shifted x, so reset vLengthSquared so that it is not used as early out for GetClosest
-        v_len_sq.value = Number.MAX_VALUE;
-
-        // We allow rebuilding the simplex once after x changes because the simplex was built
-        // for another x and numerical round off builds up as you keep adding points to an
-        // existing simplex
-        allowRestart = true;
-      }
-
-      // Add p to set P: P = P U {p}
-      ps[this.numPoints].copy(p);
-      ++this.numPoints;
-
-      // Calculate Y = {x} - P
-      for (let i = 0; i < this.numPoints; ++i) {
-        ys[i].subtractVectors(x, ps[i]);
-      }
-
-      // Determine the new closest point from Y to origin
-      const set = { value: 0 };
-      const found = getClosest(v_len_sq.value, v, v_len_sq, set, false, this.numPoints, ys);
-
-      if (!found) {
-        // Only allow 1 restart, if we still can't get a closest point
-        // we're so close that we return this as a hit
-        if (!allowRestart) {
-          break;
-        }
-
-        // If we fail to converge, we start again with the last point as simplex
-        allowRestart = false;
-        ps[0].copy(p);
-        this.numPoints = 1;
-        v.subtractVectors(x, p);
-        v_len_sq.value = Number.MAX_VALUE;
-        continue;
-      } else if (set.value === 0xf) {
-        // #v-ifdef DEV
-        // We're inside the tetrahedron, we have a hit (verify that length of v is 0)
-        if (v_len_sq.value !== 0.0) {
-          throw new Error("v length must be 0");
-        }
-        // #v-endif
-        break;
-      }
-
-      // Update the points P to form the new simplex
-      // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
-      this.numPoints = updatePointSetP(set.value, this.numPoints);
-
-      // Check if A and B are touching according to our tolerance
-      if (v_len_sq.value <= squaredTolerance) {
-        // logDebug("Converged");
-        break;
-      }
-
-      iterations++;
-    }
-
-    // Store hit fraction
-    ioLambda.fraction = lambda;
-    return true;
-  }
-}
-
-const v = /*@__PURE__*/ Vec3.create();