@perplexdotgg/bounce 1.0.0

package/src/collision/gjk/GjkModule.ts

@@ -0,0 +1,864 @@
+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();