@perplexdotgg/bounce 1.0.0

package/src/collision/gjk/PenetrationDepthModule.ts

@@ -0,0 +1,493 @@
+import { createClass, MonomorphType, NumberType, PropertyDefinitionMap } from "monomorph";
+import { Vec3 } from "../../math/vec3";
+import { GjkCastShapeResult, GjkClosestPoints, GjkModule } from "./GjkModule";
+import { degreesToRadians, squared } from "../../math/scalar";
+import { Isometry } from "../../math/isometry";
+import { ConvexRadiusObject, SupportShape, TransformedConvexObject } from "../../shape/Convex";
+import { EpaConvexHullBuilder, EpaConvexHullBuilderSettings } from "../epa/EpaConvexHullBuilder";
+import { assert } from "../../helpers";
+import { SupportPoints } from "./SupportPoints";
+import { Mat4 } from "../../math/mat4";
+import { NewTriangles } from "../epa/arrays";
+import { Triangle } from "../epa/structs";
+
+const d1 = /*@__PURE__*/ Vec3.create({ x: 0, y: 1, z: 0 });
+const d2 = /*@__PURE__*/ Vec3.create({ x: -1, y: -1, z: -1 });
+const d3 = /*@__PURE__*/ Vec3.create({ x: 1, y: -1, z: -1 });
+const d4 = /*@__PURE__*/ Vec3.create({ x: 0, y: -1, z: 1 });
+const axis = /*@__PURE__*/ Vec3.create();
+const rotation = /*@__PURE__*/ Mat4.create();
+const dir1 = /*@__PURE__*/ Vec3.create();
+const dir2 = /*@__PURE__*/ Vec3.create();
+const dir3 = /*@__PURE__*/ Vec3.create();
+const p2 = /*@__PURE__*/ Vec3.create();
+const q2 = /*@__PURE__*/ Vec3.create();
+const w2 = /*@__PURE__*/ Vec3.create();
+const negatedNormal = /*@__PURE__*/ Vec3.create();
+const p01 = /*@__PURE__*/ Vec3.create();
+const p02 = /*@__PURE__*/ Vec3.create();
+const q01 = /*@__PURE__*/ Vec3.create();
+const q02 = /*@__PURE__*/ Vec3.create();
+const p10 = /*@__PURE__*/ Vec3.create();
+const p12 = /*@__PURE__*/ Vec3.create();
+const q10 = /*@__PURE__*/ Vec3.create();
+const q12 = /*@__PURE__*/ Vec3.create();
+export const enum PenetrationDepthStatus {
+  NotColliding,
+  Colliding,
+  Indeterminate,
+}
+
+const penetrationDepthProps = {
+  status: NumberType(0),
+  penetrationAxis: MonomorphType(Vec3),
+  pointA: MonomorphType(Vec3),
+  pointB: MonomorphType(Vec3),
+} as const satisfies PropertyDefinitionMap;
+
+export class PenetrationDepth extends createClass<PenetrationDepth, typeof penetrationDepthProps>(
+  penetrationDepthProps
+) {}
+
+const transformed_a = /*@__PURE__*/ new TransformedConvexObject();
+const closest = /*@__PURE__*/ GjkClosestPoints.create();
+const add_convex_a = /*@__PURE__*/ ConvexRadiusObject.create();
+const add_convex_b = /*@__PURE__*/ ConvexRadiusObject.create();
+
+export class PenetrationDepthModule {
+  gjkModule: GjkModule;
+  hull: EpaConvexHullBuilder;
+  supportPoints: SupportPoints;
+  newTriangles: NewTriangles;
+  maxPointsToIncludeOriginInHull: number;
+
+  constructor(
+    gjk: GjkModule,
+    hullBuilderSettings: Partial<EpaConvexHullBuilderSettings> = {},
+    maxPointsToIncludeOriginInHull: number = 32
+  ) {
+    this.hull = new EpaConvexHullBuilder(hullBuilderSettings);
+    // #v-ifdef DEV
+    assert(
+      maxPointsToIncludeOriginInHull < this.hull.settings.maxPoints,
+      "maxPointsToIncludeOriginInHull must be less than maxPoints in hull builder settings"
+    );
+    // #v-endif
+    this.maxPointsToIncludeOriginInHull = maxPointsToIncludeOriginInHull;
+    this.gjkModule = gjk;
+    this.supportPoints = new SupportPoints(this.hull.settings.maxPoints);
+    this.hull.positions = this.supportPoints.mY;
+    this.newTriangles = new NewTriangles(this.hull.settings.maxEdgeLength);
+  }
+
+  getPenetrationDepthStepGjk(
+    result: PenetrationDepth,
+    inAExcludingConvexRadius: SupportShape,
+    inBExcludingConvexRadius: SupportShape,
+    inConvexRadiusA: number,
+    inConvexRadiusB: number,
+    inDirection: Vec3,
+    inTolerance: number
+  ): void {
+    // #v-ifdef DEV
+    // Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points
+    if (inDirection.isNearZero()) {
+      throw new Error("inDirection cannot be zero");
+    }
+
+    // #v-endif
+    // Get closest points
+    const combinedRadius = inConvexRadiusA + inConvexRadiusB;
+    const combinedRadiusSquared = squared(combinedRadius);
+
+    this.gjkModule.getClosestPoints(
+      closest,
+      inAExcludingConvexRadius,
+      inBExcludingConvexRadius,
+      inTolerance,
+      combinedRadiusSquared,
+      inDirection
+    );
+    result.pointA.copy(closest.pointA);
+    result.pointB.copy(closest.pointB);
+    result.penetrationAxis.copy(closest.penetrationAxis);
+
+    if (closest.squaredDistance > combinedRadiusSquared) {
+      // No collision
+      result.status = PenetrationDepthStatus.NotColliding;
+      return;
+    }
+
+    if (closest.squaredDistance > 0.0) {
+      // Collision within convex radius, adjust points for convex radius
+      const vLength = Math.sqrt(closest.squaredDistance); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX
+      result.pointA.addScaled(result.penetrationAxis, inConvexRadiusA / vLength);
+      result.pointB.addScaled(result.penetrationAxis, -inConvexRadiusB / vLength);
+      result.status = PenetrationDepthStatus.Colliding;
+      return;
+    }
+
+    result.status = PenetrationDepthStatus.Indeterminate;
+  }
+
+  // TODO: fix usage of inPenetrationTolerance
+  castShape(
+    result: GjkCastShapeResult,
+    inStart: Isometry,
+    inDirection: Vec3,
+    inCollisionTolerance: number,
+    inPenetrationTolerance: number,
+    inA: SupportShape,
+    inB: SupportShape,
+    inConvexRadiusA: number,
+    inConvexRadiusB: number,
+    inReturnDeepestPoint: boolean
+  ): void {
+    // First determine if there's a collision at all
+    this.gjkModule.castShape(
+      result,
+      inStart,
+      inDirection,
+      inCollisionTolerance,
+      inA,
+      inB,
+      inConvexRadiusA,
+      inConvexRadiusB
+    );
+    // ioCollector.addGjkCastShapeResult(result);
+    if (result.isHitFound === false) {
+      return;
+    }
+
+    // When our contact normal is too small, we don't have an accurate result
+    const contact_normal_invalid = result.separatingAxis.isNearZero(squared(inCollisionTolerance));
+
+    if (
+      inReturnDeepestPoint &&
+      result.lambda === 0.0 && // Only when lambda = 0 we can have the bodies overlap
+      (inConvexRadiusA + inConvexRadiusB === 0.0 || // When no convex radius was provided we can never trust contact points at lambda = 0
+        contact_normal_invalid)
+    ) {
+      // If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point
+      add_convex_a.setData(inConvexRadiusA, inA);
+      add_convex_b.setData(inConvexRadiusB, inB);
+      transformed_a.setData(inStart, add_convex_a);
+
+      // TODO: replace epa for imp
+      // if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
+      // return false;
+      result.isHitFound = false;
+      return;
+    } else if (contact_normal_invalid) {
+      // If we weren't able to calculate a contact normal, use the cast direction instead
+      result.separatingAxis.copy(inDirection);
+    }
+
+    // vec3.normalize(result.separatingAxis, result.separatingAxis);
+
+    result.isHitFound = true;
+    return;
+  }
+
+  getPenetrationDepthStepEPA(
+    inAIncludingConvexRadius: SupportShape,
+    inBIncludingConvexRadius: SupportShape,
+    inTolerance: number,
+    outV: Vec3,
+    outPointA: Vec3,
+    outPointB: Vec3
+  ): boolean {
+    // #v-ifdef DEV
+    assert(inTolerance >= Number.EPSILON, "inTolerance must be greater than or equal to FLT_EPSILON");
+    // #v-endif
+
+    // fetch the simplex from the gjk algorithm
+    const support_points = this.supportPoints;
+    support_points.clear();
+    this.gjkModule.getClosestPointsSimplex(support_points);
+
+    // Fill up the amount of support points to 4
+    switch (support_points.mY.size()) {
+      case 1: {
+        // #v-ifdef DEV
+        // 1 vertex, which must be at the origin, which is useless for our purpose
+        assert(
+          support_points.mY.values[0].isNearZero(squared(this.gjkModule.tolerance)),
+          "only support point Y[0] must not be near zero"
+        );
+        // #v-endif
+        support_points.popBack();
+
+        const p1 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, d1);
+        const p2 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, d2);
+        const p3 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, d3);
+        const p4 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, d4);
+
+        // #v-ifdef DEV
+        assert(p1 === 0, "support point index 0 must be added first");
+        assert(p2 === 1, "support point index 1 must be added second");
+        assert(p3 === 2, "support point index 2 must be added third");
+        assert(p4 === 3, "support point index 3 must be added fourth");
+        // #v-endif
+        break;
+      }
+
+      case 2: {
+        // Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments
+
+        axis.subtractVectors(support_points.mY.values[1], support_points.mY.values[0]).normalize();
+
+        rotation.fromAxisAngle(axis, degreesToRadians(120.0));
+
+        dir1.computeNormalizedPerpendicular(axis);
+        rotation.multiply3x3(dir2, dir1);
+        rotation.multiply3x3(dir3, dir2);
+
+        const p1 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1);
+        const p2 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2);
+        const p3 = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3);
+
+        // #v-ifdef DEV
+        assert(p1 === 2, "support point index 2 must be added first");
+        assert(p2 === 3, "support point index 3 must be added second");
+        assert(p3 === 4, "support point index 4 must be added third");
+        // #v-endif
+        break;
+      }
+
+      case 3:
+      case 4: {
+        // We already have enough points
+        break;
+      }
+    }
+
+    // Create hull out of the initial points
+    // #v-ifdef DEV
+    assert(support_points.mY.size() >= 3, "support points must have at least 3 points");
+    // #v-endif
+    const hull = this.hull;
+    hull.initialize(0, 1, 2);
+    for (let i = 3; i < support_points.mY.size(); ++i) {
+      const distSq: { value: number } = { value: -1 };
+      const t = hull.findFacingTriangle(support_points.mY.values[i], distSq);
+      if (t !== null) {
+        const newTriangles = this.newTriangles;
+        newTriangles.clear();
+
+        if (!hull.addPoint(t, i, Number.MAX_VALUE, newTriangles)) {
+          // We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already.
+          // Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate.
+          return false;
+        }
+      }
+    }
+
+    // Loop until we are sure that the origin is inside the hull
+    while (true) {
+      // Get the next closest triangle
+      const t = hull.peekClosestTriangleInQueue();
+
+      // #v-ifdef DEV
+      // assert
+      if (!t) {
+        throw new Error("t must not be null");
+      }
+      // #v-endif
+
+      // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+      if (t.removed) {
+        hull.popClosestTriangleFromQueue();
+
+        // If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+        if (!hull.hasNextTriangle()) {
+          return false;
+        }
+
+        hull.freeTriangle(t);
+        continue;
+      }
+
+      // If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm
+      if (t.closestLengthSq >= 0.0) {
+        break;
+      }
+
+      // Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue)
+      hull.popClosestTriangleFromQueue();
+
+      // Add a support point to get the origin inside the hull
+      const newIndex = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, t.normal);
+      const w = support_points.mY.values[newIndex];
+
+      // Add the point to the hull, if we fail we terminate and report no collision
+      const newTriangles = this.newTriangles;
+      newTriangles.clear();
+      if (!t.isFacing(w) || !hull.addPoint(t, newIndex, Number.MAX_VALUE, newTriangles)) {
+        return false;
+      }
+
+      // The triangle is facing the support point "w" and can now be safely removed
+      // #v-ifdef DEV
+      assert(t.removed, "triangle must be removed after adding a new point");
+      // #v-endif
+      hull.freeTriangle(t);
+
+      // If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+      if (!hull.hasNextTriangle() || support_points.mY.size() >= this.maxPointsToIncludeOriginInHull) {
+        return false;
+      }
+    }
+
+    // Current closest distance to origin
+    let closestDistSq = Number.MAX_VALUE;
+
+    // Remember last good triangle
+    let last: Triangle | null = null;
+
+    // If we want to flip the penetration depth
+    let flipVSign = false;
+
+    // Loop until closest point found
+    do {
+      // Get closest triangle to the origin
+      const t = hull.popClosestTriangleFromQueue();
+
+      // #v-ifdef DEV
+      // assert
+      if (!t) {
+        throw new Error("t must not be null");
+      }
+      // #v-endif
+
+      // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+      if (t.removed) {
+        hull.freeTriangle(t);
+        continue;
+      }
+
+      // Check if next triangle is further away than closest point, we've found the closest point
+      if (t.closestLengthSq >= closestDistSq) {
+        break;
+      }
+
+      // Replace last good with this triangle
+      if (last !== null) {
+        hull.freeTriangle(last);
+      }
+      last = t;
+
+      // Add support point in direction of normal of the plane
+      // Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane)
+      // and this way we do less calculations and lose less precision
+      const newIndex = support_points.add(inAIncludingConvexRadius, inBIncludingConvexRadius, t.normal);
+      const w = support_points.mY.values[newIndex];
+
+      // Project w onto the triangle normal
+      const dot = t.normal.dot(w);
+
+      // Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by
+      // convex radius is bigger then the original shape due to inaccuracies in the shrinking process.
+      if (dot < 0.0) {
+        return false;
+      }
+
+      // Get the distance squared (along normal) to the support point
+      const distSq = squared(dot) / t.normal.squaredLength();
+
+      // If the error became small enough, we've converged
+      if (distSq - t.closestLengthSq < t.closestLengthSq * inTolerance) {
+        break;
+      }
+
+      // Keep track of the minimum distance
+      closestDistSq = Math.min(closestDistSq, distSq);
+
+      // If the triangle thinks this point is not front facing, we've reached numerical precision and we're done
+      if (!t.isFacing(w)) {
+        break;
+      }
+
+      // Add point to hull
+      const newTriangles = this.newTriangles;
+      newTriangles.clear();
+      if (!hull.addPoint(t, newIndex, closestDistSq, newTriangles)) {
+        break;
+      }
+
+      // If the hull is starting to form defects then we're reaching numerical precision and we have to stop
+      let hasDefect = false;
+      for (const nt of newTriangles) {
+        if (nt && nt.isFacingOrigin()) {
+          hasDefect = true;
+          break;
+        }
+      }
+
+      if (hasDefect) {
+        // When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle
+        // so we do an additional check to see if the penetration in the -triangle normal direction is smaller than
+        // the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth.
+
+        negatedNormal.negateVector(t.normal);
+        inAIncludingConvexRadius.computeSupport(p2, negatedNormal);
+        inBIncludingConvexRadius.computeSupport(q2, t.normal);
+        w2.subtractVectors(p2, q2);
+        const dot2 = negatedNormal.dot(w2);
+        if (dot2 < dot) {
+          // Flip the sign of the penetration depth
+          flipVSign = !flipVSign;
+        }
+        break;
+      }
+    } while (hull.hasNextTriangle() && support_points.mY.size() < this.hull.settings.maxPoints);
+
+    // Determine closest points, if last == null it means the hull was a plane so there's no penetration
+    if (last === null) {
+      return false;
+    }
+
+    // Calculate penetration by getting the vector from the origin to the closest point on the triangle:
+    // distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal|
+    outV.copy(last.normal).scale(last.centroid.dot(last.normal) / last.normal.squaredLength());
+
+    // If penetration is near zero, treat this as a non collision since we cannot find a good normal
+    if (outV.isNearZero()) {
+      return false;
+    }
+
+    // Check if we have to flip the sign of the penetration depth
+    if (flipVSign) {
+      outV.negate();
+    }
+
+    // Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B
+    const xp0 = support_points.mP.values[last.edge[0].startIndex];
+    const xp1 = support_points.mP.values[last.edge[1].startIndex];
+    const xp2 = support_points.mP.values[last.edge[2].startIndex];
+
+    const xq0 = support_points.mQ.values[last.edge[0].startIndex];
+    const xq1 = support_points.mQ.values[last.edge[1].startIndex];
+    const xq2 = support_points.mQ.values[last.edge[2].startIndex];
+
+    if (last.lambdaRelativeTo0) {
+      p01.subtractVectors(xp1, xp0);
+      p02.subtractVectors(xp2, xp0);
+      q01.subtractVectors(xq1, xq0);
+      q02.subtractVectors(xq2, xq0);
+
+      outPointA.copy(xp0).addScaled(p01, last.lambda[0]).addScaled(p02, last.lambda[1]);
+      outPointB.copy(xq0).addScaled(q01, last.lambda[0]).addScaled(q02, last.lambda[1]);
+    } else {
+      p10.subtractVectors(xp0, xp1);
+      p12.subtractVectors(xp2, xp1);
+      q10.subtractVectors(xq0, xq1);
+      q12.subtractVectors(xq2, xq1);
+
+      outPointA.copy(xp1).addScaled(p10, last.lambda[0]).addScaled(p12, last.lambda[1]);
+      outPointB.copy(xq1).addScaled(q10, last.lambda[0]).addScaled(q12, last.lambda[1]);
+    }
+
+    return true;
+  }
+}

package/src/collision/gjk/SupportPoints.ts

@@ -0,0 +1,50 @@
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+import { Points } from "../epa/arrays";
+
+const negatedDirection = /*@__PURE__*/ Vec3.create();
+
+const p = /*@__PURE__*/ Vec3.create();
+const q = /*@__PURE__*/ Vec3.create();
+const y = /*@__PURE__*/ Vec3.create();
+
+export class SupportPoints {
+  mY: Points;
+  mP: Points;
+  mQ: Points;
+
+  constructor(capacity: number) {
+    this.mY = new Points(capacity);
+    this.mP = new Points(capacity);
+    this.mQ = new Points(capacity);
+  }
+
+  clear() {
+    this.mY.clear();
+    this.mP.clear();
+    this.mQ.clear();
+  }
+
+  // Calculate and add new support point to the list of points
+  add(inA: SupportShape, inB: SupportShape, inDirection: Vec3) {
+    negatedDirection.negateVector(inDirection);
+
+    inA.computeSupport(p, inDirection);
+    inB.computeSupport(q, negatedDirection);
+    y.subtractVectors(p, q);
+
+    // Store new point
+    this.mY.pushBackCopy(y);
+    this.mP.pushBackCopy(p);
+    this.mQ.pushBackCopy(q);
+
+    // return the index of the new point
+    return this.mY.size() - 1;
+  }
+
+  popBack() {
+    this.mY.popBack();
+    this.mP.popBack();
+    this.mQ.popBack();
+  }
+}

package/src/collision/imp/MinkowskiDifference.ts

@@ -0,0 +1,36 @@
+import { createClass, MonomorphType, PropertyDefinitionMap } from "monomorph";
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+
+const directionA = /*@__PURE__*/ Vec3.create();
+const directionB = /*@__PURE__*/ Vec3.create();
+const supportPointA = /*@__PURE__*/ Vec3.create();
+const supportPointB = /*@__PURE__*/ Vec3.create();
+
+const minkowskiDifferenceProps = {
+  direction: MonomorphType(Vec3),
+  point: MonomorphType(Vec3),
+  pointA: MonomorphType(Vec3),
+  pointB: MonomorphType(Vec3),
+} as const satisfies PropertyDefinitionMap;
+
+export class MinkowskiDifference extends createClass<MinkowskiDifference, typeof minkowskiDifferenceProps>(
+  minkowskiDifferenceProps
+) {
+  get isOriginBeyondSupportPoint(): boolean {
+    return this.point.dot(this.direction) < 0;
+  }
+
+  getSupportPointOnMinkowskiDifference(shapeA: SupportShape, shapeB: SupportShape, direction: Vec3): void {
+    directionA.copy(direction);
+    directionB.negateVector(direction);
+
+    shapeA.computeSupport(supportPointA, directionA);
+    shapeB.computeSupport(supportPointB, directionB);
+
+    this.direction.copy(direction);
+    this.point.subtractVectors(supportPointA, supportPointB);
+    this.pointA.copy(supportPointA);
+    this.pointB.copy(supportPointB);
+  }
+}

package/src/collision/imp/computeExploredDistanceLowerUpperBound.ts

@@ -0,0 +1,40 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+
+const d = /*@__PURE__*/ Vec3.create();
+
+export function computeExploredDistanceLowerUpperBound(
+  result: [number, number],
+  ray_direction: Vec3,
+  v1: Vec3,
+  v2: Vec3,
+  v3: Vec3,
+  v123_normal: Vec3,
+  o_to_v1_dot_v123_normal: number,
+  o_to_v4_dot_v_123_normal: number
+): void {
+  d.copy(ray_direction);
+  // #v-ifdef DEV
+  assert(d.isUnitVector(), "d must be a unit vector");
+  assert(v123_normal.isUnitVector(), "v123_normal must be a unit vector");
+  // #v-endif
+  const v123_normal_dot_d = v123_normal.dot(d);
+  if (Math.abs(v123_normal_dot_d) <= 0) {
+    const o_to_v1_dot_d = v1.dot(d);
+    const o_to_v2_dot_d = v2.dot(d);
+    const o_to_v3_dot_d = v3.dot(d);
+    result[0] = Math.max(o_to_v1_dot_d, o_to_v2_dot_d, o_to_v3_dot_d);
+    result[1] = result[0];
+    return;
+  }
+
+  // The case we can make a division
+  result[0] = o_to_v1_dot_v123_normal / v123_normal_dot_d;
+  result[1] = o_to_v4_dot_v_123_normal / v123_normal_dot_d;
+  // #v-ifdef DEV
+  // assert(
+  //   result[1] >= result[0],
+  //   `upper_bound must be >= lower_bound, got lower bound ${result[0]} and upper bound ${result[1]} instead`
+  // );
+  // #v-endif
+}

package/src/collision/imp/finalizeImpResult.ts

@@ -0,0 +1,69 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+import { MinkowskiDifference } from "./MinkowskiDifference";
+import { ImpResult } from "./imp";
+
+const d = /*@__PURE__*/ Vec3.create();
+const support1 = /*@__PURE__*/ MinkowskiDifference.create();
+const support2 = /*@__PURE__*/ MinkowskiDifference.create();
+const support3 = /*@__PURE__*/ MinkowskiDifference.create();
+const o_projected = /*@__PURE__*/ Vec3.create();
+const v1_to_v2 = /*@__PURE__*/ Vec3.create();
+const v1_to_v3 = /*@__PURE__*/ Vec3.create();
+const s1s2s3_plane_normal = /*@__PURE__*/ Vec3.create();
+const o_projected_to_v1 = /*@__PURE__*/ Vec3.create();
+const cross_v1_to_v3_o_projected_to_v1 = /*@__PURE__*/ Vec3.create();
+const cross_v1_to_v2_o_projected_to_v1 = /*@__PURE__*/ Vec3.create();
+
+export function finalizeImpResult(
+  penetration_data: ImpResult,
+  shapeA: SupportShape,
+  shapeB: SupportShape,
+  ray_direction: Vec3,
+  v1: Vec3,
+  v2: Vec3,
+  v3: Vec3,
+  v1_dir: Vec3,
+  v2_dir: Vec3,
+  v3_dir: Vec3,
+  v123_normal: Vec3,
+  [distance_lb, distance_ub]: [number, number]
+): void {
+  support1.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v1_dir);
+  support2.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v2_dir);
+  support3.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v3_dir);
+  v1.copy(support1.point);
+  v2.copy(support2.point);
+  v3.copy(support3.point);
+
+  d.copy(ray_direction);
+  penetration_data.minimum_penetration = distance_ub;
+  penetration_data.penetration_direction.copy(d);
+
+  // Point in triangle distance is just lb
+  // #v-ifdef DEV
+  assert(Math.abs(d.dot(v123_normal)) > 0, "d and v123_normal must be in same direction");
+  // #v-endif
+  o_projected.scaleVector(d, distance_lb);
+  v1_to_v2.subtractVectors(v2, v1);
+  v1_to_v3.subtractVectors(v3, v1);
+  s1s2s3_plane_normal.crossVectors(v1_to_v2, v1_to_v3);
+  const area = s1s2s3_plane_normal.length();
+  o_projected_to_v1.subtractVectors(v1, o_projected);
+  cross_v1_to_v3_o_projected_to_v1.crossVectors(v1_to_v3, o_projected_to_v1);
+  cross_v1_to_v2_o_projected_to_v1.crossVectors(v1_to_v2, o_projected_to_v1);
+  const s2_weight = cross_v1_to_v3_o_projected_to_v1.length() / area;
+  const s3_weight = cross_v1_to_v2_o_projected_to_v1.length() / area;
+  const s1_weight = 1 - s2_weight - s3_weight;
+
+  penetration_data.worldContactPointA.zero();
+  penetration_data.worldContactPointA.addScaled(support1.pointA, s1_weight);
+  penetration_data.worldContactPointA.addScaled(support2.pointA, s2_weight);
+  penetration_data.worldContactPointA.addScaled(support3.pointA, s3_weight);
+
+  penetration_data.worldContactPointB.zero();
+  penetration_data.worldContactPointB.addScaled(support1.pointB, s1_weight);
+  penetration_data.worldContactPointB.addScaled(support2.pointB, s2_weight);
+  penetration_data.worldContactPointB.addScaled(support3.pointB, s3_weight);
+}