@perplexdotgg/bounce 1.0.0

package/src/collision/cast/castConvexVsConvex.ts

@@ -0,0 +1,213 @@
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { ConvexWithcomputeSupportShape, SupportMode } from "../../shape/Convex";
+import { BackFaceMode } from "../collide/collide";
+import { GjkCastShapeResult } from "../gjk/GjkModule";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastResult, CastSettings } from "./cast";
+
+const negatedOffset = /*@__PURE__*/ Vec3.create();
+const offsetIsometryA = /*@__PURE__*/ Isometry.create();
+const offsetIsometryB = /*@__PURE__*/ Isometry.create();
+const inverseOffsetIsometryB = /*@__PURE__*/ Isometry.create();
+const offsetIsometryAToB = /*@__PURE__*/ Isometry.create();
+const displacementInB = /*@__PURE__*/ Vec3.create();
+
+export function castConvexVsConvex(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexWithcomputeSupportShape,
+  inShapeB: ConvexWithcomputeSupportShape,
+  isometryA: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacement: Vec3,
+  offset: Vec3,
+  inSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+): void {
+  // translate A and B by the offset, for example to move them both to origin
+  negatedOffset.negateVector(offset);
+  offsetIsometryA.matrix.postTranslatedMatrix(isometryA.matrix, negatedOffset);
+  offsetIsometryB.matrix.postTranslatedMatrix(isometryB.matrix, negatedOffset);
+
+  // get a transformA in the space of B
+  inverseOffsetIsometryB.matrix.invertMatrix(offsetIsometryB.matrix);
+  offsetIsometryAToB.matrix.multiplyMatrices(inverseOffsetIsometryB.matrix, offsetIsometryA.matrix);
+
+  // get displacement in the space of B
+  inverseOffsetIsometryB.matrix.multiply3x3(displacementInB, displacement);
+
+  castConvexVsConvexLocal(
+    penetrationDepthModule,
+    ioCollector,
+    inShapeA,
+    inShapeB,
+    offsetIsometryAToB,
+    offsetIsometryB,
+    inScaleA,
+    inScaleB,
+    displacementInB,
+    inSettings,
+    preferredNormalBDirection,
+    bodyA,
+    bodyB
+  );
+}
+
+const contact_normal_world = /*@__PURE__*/ Vec3.create();
+const epaResult = /*@__PURE__*/ GjkCastShapeResult.create();
+const result = /*@__PURE__*/ CastResult.create();
+
+// cast in B space, return in world space
+export function castConvexVsConvexLocal(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexWithcomputeSupportShape,
+  inShapeB: ConvexWithcomputeSupportShape,
+  isometryAToB: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacementInB: Vec3,
+  inShapeCastSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null,
+  subShapeIdA: number = 0,
+  subShapeIdB: number = 0
+): void {
+  // Only supported for convex shapes
+  const cast_shape = inShapeA;
+
+  const shape = inShapeB;
+
+  const support_mode = inShapeCastSettings.useShrunkenShapeAndConvexRadius
+    ? SupportMode.ExcludeConvexRadius
+    : SupportMode.IncludeConvexRadius;
+
+  // Create support function for shape to cast
+  const cast_support = cast_shape.computeSupportShape(support_mode, inScaleA);
+
+  // Create support function for target shape
+  // SOURCE
+  const target_support = shape.computeSupportShape(support_mode, inScaleB);
+  const shapeB = target_support;
+  // SOURCE
+  // CHANGE
+  // const target_support = shape.computeSupportShape(support_mode, inScale);
+  // this.transformed_b.set(inCenterOfMassTransform2, target_support);
+  // const shapeB = this.transformed_b;
+  // CHANGE
+
+  // debugger;
+
+  // Do a raycast against the result
+  epaResult.lambda = ioCollector.getEarlyOutFraction();
+  penetrationDepthModule.castShape(
+    epaResult,
+    isometryAToB,
+    displacementInB,
+    inShapeCastSettings.collisionTolerance,
+    inShapeCastSettings.penetrationTolerance,
+    cast_support,
+    shapeB,
+    cast_support.getConvexRadius(),
+    target_support.getConvexRadius(),
+    inShapeCastSettings.returnDeepestPoint
+  );
+
+  const condition =
+    epaResult.isHitFound === true &&
+    (inShapeCastSettings.backFaceModeConvex === BackFaceMode.CollideWithBackFaces ||
+      epaResult.separatingAxis.dot(displacementInB) > 0);
+  // const condition = epaResult.isHitFound === true;
+  if (condition) {
+    // SOURCE
+    // Convert to world space
+    epaResult.pointA.transformByMat4(isometryB.matrix);
+    epaResult.pointB.transformByMat4(isometryB.matrix);
+    isometryB.matrix.multiply3x3(contact_normal_world, epaResult.separatingAxis);
+    // SOURCE
+    // CHANGE
+    //
+    // CHANGE
+
+    // @ts-ignore
+    result.bodyA = bodyA;
+    // @ts-ignore
+    result.bodyB = bodyB;
+    result.subShapeIdA = subShapeIdA;
+    result.subShapeIdB = subShapeIdB;
+    result.fraction = epaResult.lambda;
+    result.isBackFaceHit = false;
+    result.contactPointA.copy(epaResult.pointA);
+    result.contactPointB.copy(epaResult.pointB);
+    // result.bodyId2 = ioCollector.bodyId2;
+
+    // SOURCE
+    result.normalA.copy(contact_normal_world);
+    result.normalB.negateVector(contact_normal_world);
+    // SOURCE
+    // CHANGE
+    // result.penetrationAxis.copy(epaResult.separatingAxis);
+    // CHANGE
+
+    result.penetration = epaResult.pointA.distance(epaResult.pointB);
+
+    // Early out if this hit is deeper than the collector's early out value
+    if (epaResult.lambda === 0 && -result.penetration >= ioCollector.getEarlyOutFraction()) {
+      ioCollector.addMiss();
+      return;
+    }
+
+    // // TODO: Gather faces
+    // if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+    // {
+    //   // Get supporting face of shape 1
+    //   Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart;
+    //   transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection);
+    //   cast_shape->computeSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
+
+    //   // Get supporting face of shape 2
+    //   shape->computeSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face);
+    // }
+
+    // adjust normal if preferred normal B direction is provided
+    // if (preferredNormalBDirection !== undefined) {
+    //   // compare normalB to preferredNormalBDirection component-wise
+    //   const difference = Vec3.global(this.world, "CastModule.castConvexVsConvexLocal.difference");
+    //   vec3.sub(difference, result.normalB, preferredNormalBDirection);
+    //   if (difference.isNearZero() === false) {
+    //     result.normalB.copy(preferredNormalBDirection);
+    //     vec3.negate(result.normalA, preferredNormalBDirection);
+    //   }
+    // }
+
+    // // exit if hit is shallower than the collector's current hit
+    // if (result.fraction >= ioCollector.getEarlyOutFraction()) {
+    //   ioCollector.addMiss();
+    //   return;
+    // }
+
+    // // flip normals
+    // vec3.negate(result.normalA, result.normalA);
+    // vec3.negate(result.normalB, result.normalB);
+
+    // always align the normals with the preferred normal direction
+    if (preferredNormalBDirection !== undefined) {
+      result.normalB.copy(preferredNormalBDirection);
+      result.normalA.negateVector(preferredNormalBDirection);
+    }
+
+    ioCollector.earlyOutFraction = result.fraction;
+
+    ioCollector.addHit(result);
+  } else {
+    ioCollector.addMiss();
+  }
+}

package/src/collision/cast/castConvexVsHeightMap.ts

@@ -0,0 +1,73 @@
+import { Isometry } from "../../math/isometry";
+import { Mat3 } from "../../math/mat3";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { Aabb } from "../../shape/Aabb";
+import { HeightMap } from "../../shape/HeightMap";
+import { Ray } from "../../shape/Ray";
+import { ConvexShape } from "../../shape/Shape";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { HeightMapCaster } from "../HeightMapCaster";
+import { TriangleCaster } from "../TriangleCaster";
+import { CastCollector, CastSettings } from "./cast";
+
+const isometryWorldToB = /*@__PURE__*/ Isometry.create();
+const isometryAToB = /*@__PURE__*/ Isometry.create();
+const shapeABoundsInBSpace = /*@__PURE__*/ Aabb.create();
+const displacementAInBSpace = /*@__PURE__*/ Vec3.create();
+const rotateWorldToB = /*@__PURE__*/ Mat3.create();
+const ray = /*@__PURE__*/ Ray.create();
+const halfExtents = /*@__PURE__*/ Vec3.create();
+const heightMapCaster = /*@__PURE__*/ new HeightMapCaster(new TriangleCaster());
+
+export function castConvexVsHeightMap(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexShape,
+  inShapeB: HeightMap,
+  isometryA: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacement: Vec3,
+  offset: Vec3,
+  inSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+): void {
+  heightMapCaster.triangleCaster.initialize(
+    inSettings,
+    ioCollector,
+    inShapeA,
+    isometryA,
+    isometryB,
+    displacement,
+    offset
+  );
+
+  isometryWorldToB.matrix.invertMatrix(isometryB.matrix);
+
+  // a to world -> world to b
+  isometryAToB.matrix.multiplyMatrices(isometryA.matrix, isometryWorldToB.matrix);
+
+  shapeABoundsInBSpace.copy(inShapeA.computedAabb);
+  shapeABoundsInBSpace.transform(isometryAToB);
+
+  rotateWorldToB.fromMat4(isometryB.matrix);
+  displacementAInBSpace.transformVectorFromMat3(displacement, rotateWorldToB);
+
+  // aabb cast is done by the following method
+  // 1. shrink the shape aabb by its own extents down to a point
+  // 2. expand the block aabb by the extents of the shape aabb
+  // 3. cast the point by the displacement against the expanded block aabb (raycast vs aabb test)
+
+  shapeABoundsInBSpace.computeCentroid(ray.origin);
+  ray.direction.normalizeVector(displacementAInBSpace);
+  ray.length = displacementAInBSpace.length();
+
+  shapeABoundsInBSpace.computeHalfExtents(halfExtents);
+
+  heightMapCaster.initialize(halfExtents, ray, bodyA, bodyB);
+  inShapeB.walkHeightMap(heightMapCaster);
+}

package/src/collision/cast/castConvexVsTriangleMesh.ts

@@ -0,0 +1,56 @@
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { Aabb } from "../../shape/Aabb";
+import { ConvexShape } from "../../shape/Shape";
+import { Triangle } from "../../shape/Triangle";
+import { TriangleMesh } from "../../shape/TriangleMesh";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { TriangleCaster } from "../TriangleCaster";
+import { CastCollector, CastSettings } from "./cast";
+
+const isometryWorldToB = /*@__PURE__*/ Isometry.create();
+const isometryAToB = /*@__PURE__*/ Isometry.create();
+const boundsA = /*@__PURE__*/ Aabb.create();
+const transformedDisplacement = /*@__PURE__*/ Vec3.create();
+const triangleCaster = /*@__PURE__*/ new TriangleCaster();
+
+function onHit(triangle: Triangle) {
+  triangleCaster.cast(triangle, triangle.activeEdges, triangle.subShapeId, triangleCaster.bodyA, triangleCaster.bodyB);
+  return false;
+}
+
+export function castConvexVsTriangleMesh(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexShape,
+  inShapeB: TriangleMesh,
+  isometryA: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacement: Vec3,
+  offset: Vec3,
+  inSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+): void {
+  triangleCaster.initialize(
+    inSettings,
+    ioCollector,
+    inShapeA,
+    isometryA,
+    isometryB,
+    displacement,
+    offset,
+    bodyA,
+    bodyB
+  );
+  isometryWorldToB.matrix.invertMatrix(isometryB.matrix);
+  isometryAToB.matrix.multiplyMatrices(isometryWorldToB.matrix, isometryA.matrix);
+  boundsA.copy(inShapeA.computedAabb);
+  boundsA.transform(isometryAToB);
+  isometryWorldToB.matrix.multiply3x3(transformedDisplacement, displacement);
+  inShapeB.bvh!.castAabb(onHit, boundsA, transformedDisplacement);
+}

package/src/collision/cast/castRayVsCompound.ts

@@ -0,0 +1,44 @@
+import { BasicTransform, ConvexShape } from "../..";
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { CompoundShape } from "../../shape/CompoundShape";
+import { ConvexWithcomputeSupportShape, SupportMode } from "../../shape/Convex";
+import { Ray } from "../../shape/Ray";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastResult, RayCastSettings } from "./cast";
+import { castRayVsConvex } from "./castRayVsConvex";
+
+export function castRayVsCompound(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inRay: Ray,
+  inShapeB: CompoundShape,
+  isometryB: Isometry,
+  inScaleB: number,
+  offset: Vec3,
+  inSettings: RayCastSettings,
+  bodyB: Body | null = null
+) {
+  for (const shapeObj of inShapeB.shapes) {
+    const shape = shapeObj.shape as ConvexShape;
+    const transform = shapeObj.transform as BasicTransform;
+
+    transformSubShapeBToB.fromRotationAndTranslation(transform.rotation, transform.position);
+    transformSubShapeBToWorld.matrix.multiplyMatrices(isometryB.matrix, transformSubShapeBToB.matrix);
+    castRayVsConvex(
+      penetrationDepthModule,
+      ioCollector,
+      inRay,
+      shape,
+      transformSubShapeBToWorld,
+      inScaleB,
+      offset,
+      inSettings,
+      bodyB
+    );
+  }
+}
+
+const transformSubShapeBToB = /*@__PURE__*/ Isometry.create();
+const transformSubShapeBToWorld = /*@__PURE__*/ Isometry.create();

package/src/collision/cast/castRayVsConvex.ts

@@ -0,0 +1,45 @@
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { ConvexWithcomputeSupportShape, SupportMode } from "../../shape/Convex";
+import { Ray } from "../../shape/Ray";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastResult, RayCastSettings } from "./cast";
+
+export function castRayVsConvex(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inRay: Ray,
+  inShapeB: ConvexWithcomputeSupportShape,
+  isometryB: Isometry,
+  inScaleB: number,
+  offset: Vec3,
+  inSettings: RayCastSettings,
+  bodyB: Body | null = null
+) {
+  const convexB = inShapeB.computeSupportShape(SupportMode.IncludeConvexRadius, 1);
+  raycast.fraction = ioCollector.getEarlyOutFraction();
+  worldToB.matrix.invertMatrix(isometryB.matrix);
+  transformedRay.copy(inRay).transform(worldToB);
+  transformedRay.direction.scale(transformedRay.length);
+  const hitFound = penetrationDepthModule.gjkModule.castRay(
+    transformedRay.origin,
+    transformedRay.direction,
+    1e-3,
+    convexB,
+    raycast
+  );
+  if (hitFound) {
+    result.hasContact = true;
+    result.bodyB = bodyB;
+    result.fraction = raycast.fraction;
+    ioCollector.addHit(result);
+  } else {
+    ioCollector.addMiss();
+  }
+}
+
+const result = /*@__PURE__*/ CastResult.create();
+const raycast = { fraction: 0 };
+const transformedRay = /*@__PURE__*/ Ray.create();
+const worldToB = /*@__PURE__*/ Isometry.create();

package/src/collision/cast/castRayVsHeightMap.ts

@@ -0,0 +1,58 @@
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { HeightMap } from "../../shape/HeightMap";
+import { Ray } from "../../shape/Ray";
+import { Triangle } from "../../shape/Triangle";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastResult, RayCastSettings } from "./cast";
+
+let collector: CastCollector;
+let ray: Ray;
+let body: Body | null;
+
+function onHit(triangle: Triangle): boolean {
+  result.reset();
+  result.hasContact = true;
+
+  raycasterResult.fraction = 1.0;
+  raycasterResult.hit = false;
+  ray.intersectsTriangle(raycasterResult, triangle);
+
+  if (!raycasterResult.hit || raycasterResult.fraction >= collector.getEarlyOutFraction()) {
+    collector.addMiss();
+    return false;
+  }
+
+  result.hasContact = true;
+  result.bodyB = body;
+  result.fraction = raycasterResult.fraction;
+  collector.earlyOutFraction = raycasterResult.fraction;
+  collector.addHit(result);
+  return false;
+}
+
+export function castRayVsHeightMap(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inRay: Ray,
+  inShapeB: HeightMap,
+  isometryB: Isometry,
+  inScaleB: number,
+  offset: Vec3,
+  inSettings: RayCastSettings,
+  bodyB: Body | null = null
+) {
+  ioCollector.reset();
+  isometryWorldToB.matrix.invertMatrix(isometryB.matrix);
+  transformedRay.copy(inRay).transform(isometryWorldToB);
+  collector = ioCollector;
+  ray = transformedRay;
+  body = bodyB;
+  inShapeB.castRay(onHit, transformedRay);
+}
+
+const result = /*@__PURE__*/ CastResult.create();
+const raycasterResult = { fraction: 1.0, hit: false };
+const isometryWorldToB = /*@__PURE__*/ Isometry.create();
+const transformedRay = /*@__PURE__*/ Ray.create();

package/src/collision/cast/castRayVsTriangleMesh.ts

@@ -0,0 +1,58 @@
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { Ray } from "../../shape/Ray";
+import { Triangle } from "../../shape/Triangle";
+import { TriangleMesh } from "../../shape/TriangleMesh";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastResult, RayCastSettings } from "./cast";
+
+let collector: CastCollector;
+let ray: Ray;
+let body: Body | null;
+
+function onHit(triangle: Triangle): boolean {
+  result.reset();
+  result.hasContact = true;
+
+  raycasterResult.fraction = 1.0;
+  raycasterResult.hit = false;
+  ray.intersectsTriangle(raycasterResult, triangle);
+
+  if (!raycasterResult.hit || raycasterResult.fraction >= collector.getEarlyOutFraction()) {
+    collector.addMiss();
+    return false;
+  }
+
+  result.hasContact = true;
+  result.bodyB = body;
+  result.fraction = raycasterResult.fraction;
+  collector.earlyOutFraction = raycasterResult.fraction;
+  collector.addHit(result);
+  return false;
+}
+
+export function castRayVsTriangleMesh(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inRay: Ray,
+  inShapeB: TriangleMesh,
+  isometryB: Isometry,
+  inScaleB: number,
+  offset: Vec3,
+  inSettings: RayCastSettings,
+  bodyB: Body | null = null
+) {
+  ioCollector.reset();
+  isometryWorldToB.matrix.invertMatrix(isometryB.matrix);
+  transformedRay.copy(inRay).transform(isometryWorldToB);
+  collector = ioCollector;
+  ray = transformedRay;
+  body = bodyB;
+  inShapeB.bvh!.castRay(onHit, transformedRay);
+}
+
+const result = /*@__PURE__*/ CastResult.create();
+const raycasterResult = { fraction: 1.0, hit: false };
+const isometryWorldToB = /*@__PURE__*/ Isometry.create();
+const transformedRay = /*@__PURE__*/ Ray.create();

package/src/collision/closestPoints/closestPoints.ts

@@ -0,0 +1,23 @@
+import { BooleanType, createClass, MonomorphType, NumberType, PropertyDefinitionMap } from "monomorph";
+import { Vec3 } from "../../math/vec3";
+
+const closestPointResultProps = {
+  point: MonomorphType(Vec3),
+  pointSet: NumberType(0),
+} as const satisfies PropertyDefinitionMap;
+
+export class ClosestPointResult extends createClass<ClosestPointResult, typeof closestPointResultProps>(
+  closestPointResultProps
+) {}
+
+const barycentricCoordinatesResultProps = {
+  u: NumberType(0),
+  v: NumberType(0),
+  w: NumberType(0),
+  isValid: BooleanType(false),
+} as const satisfies PropertyDefinitionMap;
+
+export class BarycentricCoordinatesResult extends createClass<
+  BarycentricCoordinatesResult,
+  typeof barycentricCoordinatesResultProps
+>(barycentricCoordinatesResultProps) {}

package/src/collision/closestPoints/computeBarycentricCoordinates2d.ts

@@ -0,0 +1,32 @@
+import { Vec3 } from "../../math/vec3";
+import { BarycentricCoordinatesResult } from "./closestPoints";
+
+const ab = /*@__PURE__*/ Vec3.create();
+
+export function computeBarycentricCoordinates2d(
+  outBarycentric: BarycentricCoordinatesResult,
+  a: Vec3,
+  b: Vec3,
+  squaredTolerance: number = 1e-10
+) {
+  ab.subtractVectors(b, a);
+  const denominator = ab.squaredLength();
+  if (denominator < squaredTolerance) {
+    // Degenerate line segment, fallback to points
+    if (a.squaredLength() < b.squaredLength()) {
+      // A closest
+      outBarycentric.u = 1.0;
+      outBarycentric.v = 0.0;
+    } else {
+      // B closest
+      outBarycentric.u = 0.0;
+      outBarycentric.v = 1.0;
+    }
+    outBarycentric.isValid = false;
+    return;
+  } else {
+    outBarycentric.v = -a.dot(ab) / denominator;
+    outBarycentric.u = 1.0 - outBarycentric.v;
+  }
+  outBarycentric.isValid = true;
+}

package/src/collision/closestPoints/computeBarycentricCoordinates3d.ts

@@ -0,0 +1,81 @@
+import { Vec3 } from "../../math/vec3";
+import { BarycentricCoordinatesResult } from "./closestPoints";
+import { computeBarycentricCoordinates2d } from "./computeBarycentricCoordinates2d";
+
+const ab = /*@__PURE__*/ Vec3.create();
+const ac = /*@__PURE__*/ Vec3.create();
+const bc = /*@__PURE__*/ Vec3.create();
+const otherBarycentric = /*@__PURE__*/ BarycentricCoordinatesResult.create();
+
+export function computeBarycentricCoordinates3d(
+  outBarycentric: BarycentricCoordinatesResult,
+  a: Vec3,
+  b: Vec3,
+  c: Vec3,
+  squaredTolerance: number = 1e-10
+) {
+  ab.subtractVectors(b, a);
+  ac.subtractVectors(c, a);
+  bc.subtractVectors(c, b);
+
+  const d00 = ab.dot(ab);
+  const d11 = ac.dot(ac);
+  const d22 = bc.dot(bc);
+
+  if (d00 <= d22) {
+    // Use v0 and v1 to calculate barycentric coordinates
+    const d01 = ab.dot(ac);
+
+    const denominator = d00 * d11 - d01 * d01;
+    if (Math.abs(denominator) < 1.0e-12) {
+      // Degenerate triangle, return coordinates along longest edge
+      if (d00 > d11) {
+        computeBarycentricCoordinates2d(otherBarycentric, a, b, squaredTolerance);
+        outBarycentric.u = otherBarycentric.u;
+        outBarycentric.v = otherBarycentric.v;
+        outBarycentric.w = 0.0;
+      } else {
+        computeBarycentricCoordinates2d(otherBarycentric, a, c, squaredTolerance);
+        outBarycentric.u = otherBarycentric.u;
+        outBarycentric.w = otherBarycentric.v;
+        outBarycentric.v = 0.0;
+      }
+      outBarycentric.isValid = false;
+      return;
+    } else {
+      const a0 = a.dot(ab);
+      const a1 = a.dot(ac);
+      outBarycentric.v = (d01 * a1 - d11 * a0) / denominator;
+      outBarycentric.w = (d01 * a0 - d00 * a1) / denominator;
+      outBarycentric.u = 1.0 - outBarycentric.v - outBarycentric.w;
+    }
+  } else {
+    // Use v1 and v2 to calculate barycentric coordinates
+    const d12 = ac.dot(bc);
+
+    const denominator = d11 * d22 - d12 * d12;
+    if (Math.abs(denominator) < 1.0e-12) {
+      // Degenerate triangle, return coordinates along longest edge
+      if (d11 > d22) {
+        computeBarycentricCoordinates2d(otherBarycentric, a, c, squaredTolerance);
+        outBarycentric.u = otherBarycentric.u;
+        outBarycentric.w = otherBarycentric.v;
+        outBarycentric.v = 0.0;
+      } else {
+        computeBarycentricCoordinates2d(otherBarycentric, b, c, squaredTolerance);
+        outBarycentric.v = otherBarycentric.u;
+        outBarycentric.w = otherBarycentric.v;
+        outBarycentric.u = 0.0;
+      }
+      outBarycentric.isValid = false;
+      return;
+    } else {
+      const c1 = c.dot(ac);
+      const c2 = c.dot(bc);
+      outBarycentric.u = (d22 * c1 - d12 * c2) / denominator;
+      outBarycentric.v = (d11 * c2 - d12 * c1) / denominator;
+      outBarycentric.w = 1.0 - outBarycentric.u - outBarycentric.v;
+    }
+  }
+  outBarycentric.isValid = true;
+}