@perplexdotgg/bounce 1.0.0

package/src/physics/narrowphase/CollideBodiesModule.ts

@@ -0,0 +1,755 @@
+import { destroyAllInstancesInPool } from "../../helpers";
+import { degreesToRadians, squared } from "../../math/scalar";
+import { Vec3 } from "../../math/vec3";
+import { Body, BodyType } from "../Body";
+import {
+  ActiveEdgeMode,
+  CollectFacesMode,
+  CollisionCollector,
+  CollisionResult,
+  CollisionSettings,
+} from "../../collision/collide/collide";
+import { CollideShapesModule } from "../../collision/CollideShapesModule";
+import { ContactConstraintModule } from "../../physics/solver/ContactConstraintModule";
+import { ContactManifold, ContactManifoldPool, createContactPairKey } from "../manifold/ContactManifold";
+import { ContactManifoldModule } from "../manifold/ContactManifoldModule";
+import { SleepModule } from "../../physics/SleepModule";
+import { Shape, ShapeType } from "../../shape/Shape";
+import { Isometry } from "../../math/isometry";
+import { HeightMap } from "../../shape/HeightMap";
+import { Aabb } from "../../shape/Aabb";
+import type { World } from "../../world";
+import { CastCollector, CastResult, CastSettings, RayCastSettings } from "../../collision/cast/cast";
+import { CastShapesModule } from "../../collision/CastShapesModule";
+import { AllFlag } from "../CollisionFilter";
+import { Quat } from "../../math/quat";
+import { Ray } from "../../shape/Ray";
+import { estimateCollisionResponse, EstimateCollisionResponseResult } from "../solver/estimateCollisionResponse";
+
+const manifolds = /*@__PURE__*/ new ContactManifoldPool(32, 64);
+
+const inverseOrientationA = /*@__PURE__*/ Quat.create();
+
+export interface CollideBodiesSettings {
+  speculativeContactDistance: number;
+}
+
+const worldSpaceNormal = /*@__PURE__*/ Vec3.create();
+const negatedWorldSpaceNormal = /*@__PURE__*/ Vec3.create();
+const zeroVector = /*@__PURE__*/ Vec3.create({ x: 0, y: 0, z: 0 });
+
+const centerOfMassAInWorldSpace = /*@__PURE__*/ Vec3.create();
+const isometryWorldToB = /*@__PURE__*/ Isometry.create();
+const projectedPointInB = /*@__PURE__*/ Vec3.create();
+const projectedPointOnHeightMap = /*@__PURE__*/ Vec3.create();
+
+const isometryA = /*@__PURE__*/ Isometry.create();
+const isometryB = /*@__PURE__*/ Isometry.create();
+const offset = /*@__PURE__*/ Vec3.create();
+const negatedOffset = /*@__PURE__*/ Vec3.create();
+const projectedPosition = /*@__PURE__*/ Vec3.create();
+const centerOfMassAInB = /*@__PURE__*/ Vec3.create();
+const linearVelocityBA = /*@__PURE__*/ Vec3.create();
+
+class CollideBodiesCollector extends CollisionCollector {
+  numHits: number;
+  settings: CollideBodiesSettings;
+  contactManifoldModule: ContactManifoldModule;
+  bodyA: Body | null;
+  bodyB: Body | null;
+
+  constructor(contactManifoldModule: ContactManifoldModule) {
+    super();
+    this.numHits = 0;
+    this.settings = { speculativeContactDistance: 0.02 };
+    this.contactManifoldModule = contactManifoldModule;
+    this.bodyA = null;
+    this.bodyB = null;
+  }
+
+  init(settings: CollideBodiesSettings, bodyA: Body, bodyB: Body): this {
+    this.reset();
+    this.numHits = 0;
+    this.settings = settings;
+    this.bodyA = bodyA;
+    this.bodyB = bodyB;
+
+    destroyAllInstancesInPool(manifolds.contactManifoldPool);
+    return this;
+  }
+
+  addHit(result: CollisionResult): void {
+    this.numHits++;
+
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+
+    // get the normal
+    worldSpaceNormal.normalizeVector(result.normalA); // TODO: or B?
+
+    // check if we can add the contact point
+    const maxDeltaCosAngle = Math.cos(degreesToRadians(5));
+
+    // iterate over all manifolds
+    let reachedEndWithoutAdding = true;
+    let newManifold: ContactManifold | null = null;
+    for (const manifold of manifolds.contactManifoldPool) {
+      if (worldSpaceNormal.dot(manifold.firstWorldSpaceNormal) >= maxDeltaCosAngle) {
+        // we can add it
+        manifold.worldSpaceNormal.addVector(worldSpaceNormal);
+        manifold.penetrationDepth = Math.max(manifold.penetrationDepth, result.penetration);
+        reachedEndWithoutAdding = false;
+        newManifold = manifold;
+        break;
+      }
+    }
+
+    if (reachedEndWithoutAdding) {
+      // TODO handle trimesh
+
+      // case: full manifolds, replace the one with the least penetration depth if possible
+      if (manifolds.contactManifoldPool.length === manifolds.contactManifoldPool.maxLength) {
+        let minPenetrationDepth = Infinity;
+        // let minPenetrationDepthId = -1;
+        let minPenetrationDepthManifold: ContactManifold | null = null;
+
+        // iterate over all manifolds
+        for (const manifold of manifolds.contactManifoldPool) {
+          if (manifold.penetrationDepth < minPenetrationDepth) {
+            minPenetrationDepth = manifold.penetrationDepth;
+            minPenetrationDepthManifold = manifold;
+          }
+        }
+
+        // get the manifold with the least penetration depth
+        if (!minPenetrationDepthManifold) {
+          throw new Error("minPenetrationDepthManifold is null");
+        }
+
+        // case: result penetration is less than the minimum penetration depth, skip this result
+        if (result.penetration > minPenetrationDepth) {
+          return;
+        }
+
+        // case: replace the manifold with the least penetration depth
+        minPenetrationDepthManifold.destroy();
+        newManifold = manifolds.createContactManifold({
+          bodyA: bodyA,
+          bodyB: bodyB,
+          subShapeIdA: result.subShapeIdA,
+          subShapeIdB: result.subShapeIdB,
+          nextContactManifold: null,
+
+          baseTranslation: { x: 0, y: 0, z: 0 },
+          firstWorldSpaceNormal: worldSpaceNormal,
+          worldSpaceNormal: worldSpaceNormal,
+          penetrationDepth: result.penetration,
+          numContacts: 0,
+        });
+      }
+      // case: room to add a new manifold
+      else {
+        newManifold = manifolds.createContactManifold({
+          bodyA: bodyA,
+          bodyB: bodyB,
+          subShapeIdA: result.subShapeIdA,
+          subShapeIdB: result.subShapeIdB,
+          nextContactManifold: null,
+
+          baseTranslation: { x: 0, y: 0, z: 0 },
+          firstWorldSpaceNormal: worldSpaceNormal,
+          worldSpaceNormal: worldSpaceNormal,
+          penetrationDepth: result.penetration,
+          numContacts: 0,
+        });
+      }
+    }
+
+    if (!newManifold) {
+      throw new Error("newManifold is null");
+    }
+
+    // add contact point
+    // get the supporting faces
+    negatedWorldSpaceNormal.negateVector(worldSpaceNormal);
+    zeroVector.zero();
+
+    if (result.faceA.numVertices === 0) {
+      bodyA!.computeSupportingFace(result.faceA, result.subShapeIdA, negatedWorldSpaceNormal, zeroVector);
+    }
+
+    if (result.faceB.numVertices === 0) {
+      bodyB!.computeSupportingFace(result.faceB, result.subShapeIdB, worldSpaceNormal, zeroVector);
+    }
+
+    const penetrationTolerance = 1e-3;
+    // const penetrationToleranceSquared = squared(penetrationTolerance);
+    // const speculativeContactDistanceSquared = squared(this.settings.speculativeContactDistance);
+    // const maxContactDistancedSquared = speculativeContactDistanceSquared + penetrationToleranceSquared;
+
+    result.contactPointA.addVector(result.bodyA!.computedCenterOfMassPosition);
+    result.contactPointB.addVector(result.bodyA!.computedCenterOfMassPosition);
+    // newManifold.baseTranslation.copy(bodyA.computedCenterOfMassPosition);
+
+    this.contactManifoldModule.manifoldBetweenTwoFaces(
+      result.contactPointA,
+      result.contactPointB,
+      worldSpaceNormal, // TODO: A or B?
+      // maxContactDistancedSquared,
+      this.settings.speculativeContactDistance + 1e-3,
+      result.faceA,
+      result.faceB,
+      newManifold
+    );
+
+    // prune if more than 32 contact points
+    if (newManifold.numContacts > 32) {
+      this.contactManifoldModule.pruneContactPoints(newManifold.firstWorldSpaceNormal, newManifold);
+    }
+  }
+}
+
+export class CollideBodiesModule {
+  world: World;
+  collideShapesModule: CollideShapesModule;
+  castShapesModule: CastShapesModule;
+  contactManifoldModule: ContactManifoldModule;
+  contactConstraintModule: ContactConstraintModule;
+  sleepModule: SleepModule;
+
+  collector: CollideBodiesCollector;
+
+  constructor(
+    world: World,
+    collideShapesModule: CollideShapesModule,
+    castShapesModule: CastShapesModule,
+    contactManifoldModule: ContactManifoldModule,
+    contactConstraintModule: ContactConstraintModule,
+    sleepModule: SleepModule
+  ) {
+    this.world = world;
+    this.collideShapesModule = collideShapesModule;
+    this.castShapesModule = castShapesModule;
+    this.contactManifoldModule = contactManifoldModule;
+    this.contactConstraintModule = contactConstraintModule;
+    this.sleepModule = sleepModule;
+
+    this.collector = new CollideBodiesCollector(contactManifoldModule);
+  }
+
+  /**
+   * checks for collisions between two bodies. assumes bodyA is always dynamic. bodyB can be any type
+   */
+  collideBodies(
+    // onCollide: (manifold: ContactManifold) => void,
+    bodyA: Body,
+    bodyB: Body,
+    collideShapesSettings: CollisionSettings,
+    collideBodiesSettings: CollideBodiesSettings,
+    timeStepSizeSeconds: number,
+    isWarmStartingEnabled: boolean,
+    minVelocityForElasticContact: number
+  ) {
+    // TODO: pass use persistent contacts as a parameter?
+    let usePersistentContacts: boolean;
+    usePersistentContacts = true;
+    usePersistentContacts = false;
+    let pairHandled = false;
+    let constraintCreated = false;
+
+    if (usePersistentContacts) {
+      [pairHandled, constraintCreated] = this.contactManifoldModule.getContactsFromCache(
+        bodyA,
+        bodyB,
+        this.contactConstraintModule,
+        this.sleepModule,
+        isWarmStartingEnabled,
+        timeStepSizeSeconds,
+        minVelocityForElasticContact
+      );
+    }
+
+    // exit if pair handled by persistent contacts
+    if (pairHandled) {
+      // TODO: add bodies-in-contact for contact constraint contact baumgarte multiplier
+      return;
+    }
+
+    linearVelocityBA.subtractVectors(bodyA.linearVelocity, bodyB.linearVelocity);
+    collideShapesSettings.activeEdgeMovementDirectionX = linearVelocityBA.x;
+    collideShapesSettings.activeEdgeMovementDirectionY = linearVelocityBA.y;
+    collideShapesSettings.activeEdgeMovementDirectionZ = linearVelocityBA.z;
+    collideShapesSettings.maxSeparation = collideBodiesSettings.speculativeContactDistance;
+    // collideShapesSettings.collectFacesMode = CollectFacesMode.CollectFaces;
+    collideShapesSettings.activeEdgeMode = ActiveEdgeMode.CollideOnlyWithActive;
+
+    offset.copy(bodyA.computedCenterOfMassPosition);
+    negatedOffset.negateVector(offset);
+    isometryA.matrix.fromQuat(bodyA.orientation);
+    isometryB.fromRotationAndTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
+    isometryB.matrix.postTranslated(negatedOffset);
+
+    // TODO: handle trimesh
+    this.collector.init(collideBodiesSettings, bodyA, bodyB);
+    this.collideShapesModule.collideShapes(
+      this.collector,
+      bodyA.shape!,
+      isometryA,
+      bodyB.shape!,
+      isometryB,
+      collideShapesSettings,
+      bodyA,
+      bodyB,
+      0,
+      0
+    );
+
+    if (this.collector.numHits < 1) {
+      return;
+
+      // code below was an attempt to fix tunneling through heightmaps, but it caused some problems so it's disabled for now
+
+      // case: no contact found between shape and non-heightmap, we're really done
+      if (bodyB.shape!.type !== ShapeType.heightMap) {
+        // this.world.tracker.timeEnd("compute contact manifolds");
+        return;
+      }
+
+      if (bodyA.shape!.type === ShapeType.triangleMesh) {
+        // this.world.tracker.timeEnd("compute contact manifolds");
+        return;
+      }
+
+      // case: no contact found between shape and heightmap triangles
+      // check if bodyA center of mass is above the heightmap
+
+      projectedPosition.addScaledToVector(
+        bodyA.computedCenterOfMassPosition,
+        bodyA.linearVelocity,
+        timeStepSizeSeconds
+      );
+      centerOfMassAInWorldSpace.copy(projectedPosition);
+      isometryWorldToB.matrix.fromInverseRotationAndTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
+      projectedPointInB.transformVectorFromMat4(centerOfMassAInWorldSpace, isometryWorldToB.matrix);
+      centerOfMassAInB.transformVectorFromMat4(bodyA.computedCenterOfMassPosition, isometryWorldToB.matrix);
+
+      const interpolatedHeight = (bodyB.shape as unknown as HeightMap).computeInterpolatedHeight(projectedPointInB);
+
+      // case: bodyA center of mass is above the heightmap surface and no contact was found, we're really done
+      if (projectedPointInB.y >= interpolatedHeight) {
+        // this.world.tracker.timeEnd("compute contact manifolds");
+        return;
+      }
+
+      // case: bodyA center of mass is below the heightmap surface and no contact was found, it has tunneled through the heightmap
+
+      projectedPointOnHeightMap.copy(centerOfMassAInB);
+      projectedPointOnHeightMap.y = interpolatedHeight + 1e-1;
+      isometryWorldToB.inverseTransformPoint(projectedPointOnHeightMap); // tx from b to world
+
+      // update the broadphase. TODO: does this need to happen here?
+      // this.world.broadphaseModule.moveBody(bodyA.cachedComboId);
+      bodyA.markBodyAsDirty();
+      bodyA.computedCenterOfMassPosition.copy(projectedPointOnHeightMap);
+
+      // update the isometry
+      offset.copy(bodyA.computedCenterOfMassPosition);
+      negatedOffset.negateVector(offset);
+      isometryA.matrix.fromQuat(bodyA.orientation);
+      isometryB.fromRotationAndTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
+      isometryB.matrix.postTranslated(negatedOffset);
+
+      // re-run the collision detection (TODO: what impact does this have on any other contacts this body has made?)
+      this.collector.init(collideBodiesSettings, bodyA, bodyB);
+      this.collideShapesModule.collideShapes(
+        this.collector,
+        bodyA.shape!,
+        isometryA,
+        bodyB.shape!,
+        isometryB,
+        collideShapesSettings,
+        bodyA,
+        bodyB,
+        0,
+        0
+      );
+
+      // if still no contact, we're really done
+      if (this.collector.numHits === 0) {
+        // this.world.tracker.timeEnd("compute contact manifolds");
+        return;
+      }
+
+      // if we're here, we've applied the tunneling fix, found a contact, and should fall-through to the contact processing steps
+    }
+
+    const pair = this.contactManifoldModule.addContactPair(bodyA, bodyB);
+    // store the current relative translation to use for next frame's contact cache evaluation
+    inverseOrientationA.conjugateQuat(bodyA.orientation); // we're able to conjugate instead of invert because it's a unit quaternion
+    pair.translationAB.subtractVectors(bodyB.computedCenterOfMassPosition, bodyA.computedCenterOfMassPosition);
+    pair.translationAB.transformVectorByQuat(pair.translationAB, inverseOrientationA);
+    // TODO: add bodies-in-contact for contact constraint contact baumgarte multiplier
+
+    // iterate over all collected manifolds
+    for (const manifold of manifolds.contactManifoldPool) {
+      manifold.worldSpaceNormal.normalize();
+
+      if (manifold.numContacts > 4) {
+        this.contactManifoldModule.pruneContactPoints(manifold.worldSpaceNormal, manifold);
+      }
+
+      if (manifold.numContacts < 1) {
+        throw new Error("manifold has no contacts");
+      }
+
+      this.contactConstraintModule.addContactConstraints(
+        bodyA,
+        bodyB,
+        manifold,
+        timeStepSizeSeconds,
+        isWarmStartingEnabled,
+        minVelocityForElasticContact
+      );
+
+      // onCollide(manifold);
+    }
+  }
+
+  collideShape(
+    inShape: Shape,
+    inShapeScale: number,
+    inCenterOfMassTransform: Isometry,
+    ioCollector: CollisionCollector,
+    inCollideShapeSettings: CollisionSettings,
+    inbaseTranslation?: Vec3,
+    mask: number = AllFlag
+  ): void {
+    // TODO: handle other shapes
+    // TODO: handle non-static bodies
+    // iterate over all static bodies
+
+    // offset isometry A by the negated base offset
+    if (inbaseTranslation) {
+      offset.negateVector(inbaseTranslation);
+    } else {
+      offset.zero();
+    }
+
+    isometryA.matrix.postTranslatedMatrix(inCenterOfMassTransform.matrix, offset);
+
+    // get shapeA world bounds
+
+    boundsA.copy(inShape.computedAabb);
+    // expand the bounds by the max separation distance
+    boundsA.expand(inCollideShapeSettings ? inCollideShapeSettings.maxSeparation : 0.0);
+    // transform the bounds to world space
+    boundsA.transform(inCenterOfMassTransform);
+
+    collideShapeVisitor.initialize(
+      isometryA,
+      isometryB,
+      ioCollector,
+      this.collideShapesModule,
+      inShape,
+      offset,
+      ioCollector,
+      inCollideShapeSettings
+    );
+    const group = AllFlag; // CollisionFilter.AllFlag
+    this.world.broadphase.visitBodiesWithCollision(collideShapeVisitor, boundsA, group, mask);
+  }
+
+  castShape(
+    inShape: Shape,
+    inShapeScale: number,
+    inCenterOfMassTransform: Isometry,
+    inDisplacement: Vec3,
+    inShapeCastSettings: CastSettings,
+    inbaseTranslation: Vec3,
+    ioCollector: CastCollector,
+    mask: number = AllFlag
+    // mask: number = CollisionFilter.AllFlag
+  ) {
+    // TODO: handle other shapes
+    // TODO: handle non-static bodies
+    // iterate over all static bodies
+
+    const castModule = this.castShapesModule;
+
+    // get shapeA world bounds
+    boundsA.copy(inShape.computedAabb);
+
+    // expand the bounds by the max separation distance
+    boundsA.expand(inShapeCastSettings.collisionTolerance);
+
+    // transform the bounds to world space
+    boundsA.transform(inCenterOfMassTransform);
+
+    // the displacement is already in world space
+
+    const visitor = castShapeVisitor;
+    visitor.initialize(
+      isometryB,
+      ioCollector,
+      castModule,
+      ioCollector,
+      inShape,
+      inCenterOfMassTransform,
+      inShapeScale,
+      inDisplacement,
+      inbaseTranslation,
+      inShapeCastSettings
+    );
+    const group = AllFlag; // CollisionFilter.AllFlag
+    this.world.broadphase.visitBodiesWithCast(visitor, boundsA, inDisplacement, group, mask);
+  }
+
+  castRay(
+    onHit: (result: CastRayResult) => boolean,
+    ray: Ray,
+    settings: RayCastSettings,
+    belongsToGroups: number = AllFlag,
+    collidesWithGroups: number = AllFlag
+  ) {
+    if (settings.returnClosestOnly) {
+      const visitor = castRayVisitor;
+      const collector = castRayCollector;
+      visitor.init(ray, settings, this.castShapesModule, collector);
+      this.world.broadphase.raycastBodies(visitor, ray, belongsToGroups, collidesWithGroups);
+      if (collector.closestBody) {
+        raycastResult.body = collector.closestBody;
+        raycastResult.fraction = collector.closestFraction;
+        onHit(raycastResult);
+      }
+    }
+  }
+
+  estimateCollisionResponse(
+    result: EstimateCollisionResponseResult,
+    bodyA: Body,
+    bodyB: Body,
+    timeStepSizeSeconds: number
+  ): void {
+    result.reset();
+    const cache = this.contactManifoldModule.currentManifoldCache;
+    const key = createContactPairKey(bodyA, bodyB);
+    const pair = cache.pairMap.get(key);
+    if (!pair) {
+      return;
+    }
+    // we use just the first manifold for now
+    const manifold = pair.firstContactManifold;
+    if (!manifold) {
+      return;
+    }
+
+    estimateCollisionResponse(result, manifold, timeStepSizeSeconds);
+  }
+}
+
+const raycastResult: CastRayResult = {
+  // @ts-ignore
+  body: null,
+  fraction: Infinity,
+};
+
+export interface CastRayResult {
+  body: Body;
+  fraction: number;
+}
+
+class CastRayCollector extends CastCollector {
+  closestBody: Body | null;
+  closestFraction: number;
+
+  constructor() {
+    super();
+    this.closestBody = null;
+    this.closestFraction = Infinity;
+  }
+
+  reset() {
+    super.reset();
+    this.closestBody = null;
+    this.closestFraction = Infinity;
+    return this;
+  }
+
+  addHit(result: CastResult): void {
+    if (result.fraction < this.closestFraction) {
+      this.closestFraction = result.fraction;
+      this.closestBody = result.bodyB;
+    }
+  }
+}
+
+const castRayCollector = /*@__PURE__*/ new CastRayCollector();
+
+class CastRayBodyVisitor implements BodyVisitor {
+  shouldExit: boolean;
+  settings: RayCastSettings | null;
+  ray: Ray | null;
+  castModule: CastShapesModule | null;
+  collector: CastRayCollector | null;
+
+  constructor() {
+    this.shouldExit = false;
+    this.ray = null;
+    this.settings = null;
+    this.castModule = null;
+    this.collector = null;
+  }
+
+  init(ray: Ray, settings: RayCastSettings, castModule: CastShapesModule, collector: CastRayCollector) {
+    this.shouldExit = false;
+    this.ray = ray;
+    this.settings = settings;
+    this.castModule = castModule;
+    this.collector = collector;
+    this.collector.reset();
+  }
+
+  visit(body: Body): void {
+    isometryB.fromRotationAndTranslation(body.orientation, body.computedCenterOfMassPosition);
+    this.castModule!.castRay(this.collector!, this.ray!, body.shape!, isometryB, 1.0, zeroVector, this.settings!, body);
+  }
+}
+
+const castRayVisitor = /*@__PURE__*/ new CastRayBodyVisitor();
+
+class CollideShapeBodyVisitor {
+  shouldExit: boolean = false;
+
+  declare isometryA: Isometry;
+  declare isometryB: Isometry;
+  declare collector: CollisionCollector;
+  declare collisionModule: CollideShapesModule;
+  declare inShape: Shape;
+  declare inCollideShapeSettings: CollisionSettings;
+  declare offset: Vec3;
+  declare ioCollector: CollisionCollector;
+
+  initialize(
+    isometryA: Isometry,
+    isometryB: Isometry,
+    collector: CollisionCollector,
+    collisionModule: CollideShapesModule,
+    inShape: Shape,
+    offset: Vec3,
+    ioCollector: CollisionCollector,
+    inCollideShapeSettings: CollisionSettings
+  ): void {
+    this.isometryA = isometryA;
+    this.isometryB = isometryB;
+    this.collector = collector;
+    this.collisionModule = collisionModule;
+    this.inShape = inShape;
+    this.inCollideShapeSettings = inCollideShapeSettings;
+    this.offset = offset;
+    this.ioCollector = ioCollector;
+  }
+
+  visit(body: Body) {
+    // set the isometry, offset it by the negated base offset
+    this.isometryB.fromRotationAndTranslation(body.orientation, body.computedCenterOfMassPosition);
+    this.isometryB.matrix.postTranslatedMatrix(this.isometryB.matrix, this.offset);
+
+    // Notify collector of new body
+    this.ioCollector.body2 = body;
+
+    this.collisionModule.collideShapes(
+      this.ioCollector,
+      this.inShape,
+      this.isometryA,
+      body.shape!,
+      this.isometryB,
+      this.inCollideShapeSettings,
+      null,
+      body
+    );
+  }
+}
+
+export interface BodyVisitor {
+  shouldExit: boolean;
+  visit(body: Body): void;
+}
+
+class CastShapeBodyVisitor implements BodyVisitor {
+  shouldExit: boolean = false;
+
+  declare isometryB: Isometry;
+  declare collector: CastCollector;
+  declare castModule: CastShapesModule;
+  declare ioCollector: CastCollector;
+  declare inShape: Shape;
+  declare inCenterOfMassTransform: Isometry;
+  declare inShapeScale: number;
+  declare inDisplacement: Vec3;
+  declare inbaseTranslation: Vec3;
+  declare inShapeCastSettings: CastSettings;
+
+  initialize(
+    isometryB: Isometry,
+    collector: CastCollector,
+    castModule: CastShapesModule,
+    ioCollector: CastCollector,
+    inShape: Shape,
+    inCenterOfMassTransform: Isometry,
+    inShapeScale: number,
+    inDisplacement: Vec3,
+    inbaseTranslation: Vec3,
+    inShapeCastSettings: CastSettings
+  ): void {
+    this.isometryB = isometryB;
+    this.collector = collector;
+    this.castModule = castModule;
+    this.ioCollector = ioCollector;
+    this.inShape = inShape;
+    this.inCenterOfMassTransform = inCenterOfMassTransform;
+    this.inShapeScale = inShapeScale;
+    this.inDisplacement = inDisplacement;
+    this.inbaseTranslation = inbaseTranslation;
+    this.inShapeCastSettings = inShapeCastSettings;
+  }
+
+  visit(body: Body): void {
+    // set the isometry
+    this.isometryB.fromRotationAndTranslation(body.orientation, body.computedCenterOfMassPosition);
+
+    // Collect the transformed shape
+    // TODO: the type casting into ShapeWithSupport is not a good solution here, need to really rework the shape type system
+    // this.transformedShape.initialize(body.position, body.orientation, body.shape as ShapeWithSupport, id);
+
+    // Notify collector of new body
+    this.ioCollector.body2 = body;
+
+    this.castModule.castShapes(
+      this.ioCollector,
+      this.inShape,
+      this.inCenterOfMassTransform,
+      this.inShapeScale,
+      body.shape!,
+      this.isometryB,
+      1.0,
+      this.inDisplacement,
+      this.inbaseTranslation,
+      this.inShapeCastSettings,
+      null,
+      body
+    );
+
+    // Do narrow phase collision check
+    // this.transformedShape.castShape(inShapeCast, inShapeCastSettings, inbaseTranslation, ioCollector);
+
+    // TODO: this may not be a good translation of
+    // UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+
+    // Update early out fraction based on narrow phase collector
+    this.ioCollector.updateEarlyOutFraction(this.ioCollector.getEarlyOutFraction());
+  }
+}
+
+const boundsA = /*@__PURE__*/ Aabb.create();
+const collideShapeVisitor = /*@__PURE__*/ new CollideShapeBodyVisitor();
+const castShapeVisitor = /*@__PURE__*/ new CastShapeBodyVisitor();