@perplexdotgg/bounce 1.0.0

package/src/collision/CollideShapesModule.ts

@@ -0,0 +1,152 @@
+import { Isometry } from "../math/isometry";
+import { Shape, ShapeType } from "../shape/Shape";
+import { PenetrationDepthModule } from "./gjk/PenetrationDepthModule";
+import { CollisionCollector, CollisionSettings } from "./collide/collide";
+import { collideSphereVsSphere } from "./collide/collideSphereVsSphere";
+import { collideConvexVsConvex } from "./collide/collideConvexVsConvex";
+import { collideCompoundVsConvex } from "./collide/collideCompoundVsConvex";
+import { collideConvexVsCompound } from "./collide/collideConvexVsCompound";
+import { collideCompoundVsCompound } from "./collide/collideCompoundVsCompound";
+import { collideConvexVsHeightMap } from "./collide/collideConvexVsHeightMap";
+import { collideCompoundVsHeightMap } from "./collide/collideCompoundVsHeightMap";
+import { collideHeightMapVsConvex } from "./collide/collideHeightMapVsConvex";
+import { collideHeightMapVsCompound } from "./collide/collideHeightMapVsCompound";
+import { collideConvexVsTriangleMesh } from "./collide/collideConvexVsTriangleMesh";
+import { collideTriangleMeshVsConvex } from "./collide/collideTriangleMeshVsConvex";
+import { collideTriangleMeshVsCompound } from "./collide/collideTriangleMeshVsCompound";
+import { collideCompoundVsTriangleMesh } from "./collide/collideCompoundVsTriangleMesh";
+import { Body } from "../physics/Body";
+
+function noOp() {
+  return;
+}
+
+const collideShapesFns = {
+  [ShapeType.sphere]: {
+    [ShapeType.box]: collideConvexVsConvex,
+    [ShapeType.capsule]: collideConvexVsConvex,
+    [ShapeType.convexHull]: collideConvexVsConvex,
+    [ShapeType.compoundShape]: collideConvexVsCompound,
+    [ShapeType.cylinder]: collideConvexVsConvex,
+    [ShapeType.heightMap]: collideConvexVsHeightMap,
+    [ShapeType.sphere]: collideSphereVsSphere,
+    [ShapeType.triangleMesh]: collideConvexVsTriangleMesh,
+  },
+  [ShapeType.box]: {
+    [ShapeType.box]: collideConvexVsConvex,
+    [ShapeType.capsule]: collideConvexVsConvex,
+    [ShapeType.compoundShape]: collideConvexVsCompound,
+    [ShapeType.convexHull]: collideConvexVsConvex,
+    [ShapeType.cylinder]: collideConvexVsConvex,
+    [ShapeType.heightMap]: collideConvexVsHeightMap,
+    [ShapeType.sphere]: collideConvexVsConvex,
+    [ShapeType.triangleMesh]: collideConvexVsTriangleMesh,
+  },
+  [ShapeType.convexHull]: {
+    [ShapeType.box]: collideConvexVsConvex,
+    [ShapeType.capsule]: collideConvexVsConvex,
+    [ShapeType.compoundShape]: collideConvexVsCompound,
+    [ShapeType.convexHull]: collideConvexVsConvex,
+    [ShapeType.cylinder]: collideConvexVsConvex,
+    [ShapeType.heightMap]: collideConvexVsHeightMap,
+    [ShapeType.sphere]: collideConvexVsConvex,
+    [ShapeType.triangleMesh]: collideConvexVsTriangleMesh,
+  },
+  [ShapeType.compoundShape]: {
+    [ShapeType.box]: collideCompoundVsConvex,
+    [ShapeType.capsule]: collideCompoundVsConvex,
+    [ShapeType.compoundShape]: collideCompoundVsCompound,
+    [ShapeType.convexHull]: collideCompoundVsConvex,
+    [ShapeType.cylinder]: collideCompoundVsConvex,
+    [ShapeType.heightMap]: collideCompoundVsHeightMap,
+    [ShapeType.sphere]: collideCompoundVsConvex,
+    [ShapeType.triangleMesh]: collideCompoundVsTriangleMesh,
+  },
+  [ShapeType.cylinder]: {
+    [ShapeType.box]: collideConvexVsConvex,
+    [ShapeType.capsule]: collideConvexVsConvex,
+    [ShapeType.compoundShape]: collideConvexVsCompound,
+    [ShapeType.convexHull]: collideConvexVsConvex,
+    [ShapeType.cylinder]: collideConvexVsConvex,
+    [ShapeType.heightMap]: collideConvexVsHeightMap,
+    [ShapeType.sphere]: collideConvexVsConvex,
+    [ShapeType.triangleMesh]: collideConvexVsTriangleMesh,
+  },
+  [ShapeType.capsule]: {
+    [ShapeType.box]: collideConvexVsConvex,
+    [ShapeType.capsule]: collideConvexVsConvex,
+    [ShapeType.compoundShape]: collideConvexVsCompound,
+    [ShapeType.convexHull]: collideConvexVsConvex,
+    [ShapeType.cylinder]: collideConvexVsConvex,
+    [ShapeType.heightMap]: collideConvexVsHeightMap,
+    [ShapeType.sphere]: collideConvexVsConvex,
+    [ShapeType.triangleMesh]: collideConvexVsTriangleMesh,
+  },
+  [ShapeType.heightMap]: {
+    [ShapeType.box]: collideHeightMapVsConvex,
+    [ShapeType.capsule]: collideHeightMapVsConvex,
+    [ShapeType.compoundShape]: collideHeightMapVsCompound,
+    [ShapeType.convexHull]: collideHeightMapVsConvex,
+    [ShapeType.cylinder]: collideHeightMapVsConvex,
+    [ShapeType.heightMap]: noOp,
+    [ShapeType.sphere]: collideHeightMapVsConvex,
+    [ShapeType.triangleMesh]: noOp,
+  },
+  [ShapeType.triangleMesh]: {
+    [ShapeType.box]: collideTriangleMeshVsConvex,
+    [ShapeType.capsule]: collideTriangleMeshVsConvex,
+    [ShapeType.compoundShape]: collideTriangleMeshVsCompound,
+    [ShapeType.convexHull]: collideTriangleMeshVsConvex,
+    [ShapeType.cylinder]: collideTriangleMeshVsConvex,
+    [ShapeType.heightMap]: noOp,
+    [ShapeType.sphere]: collideTriangleMeshVsConvex,
+    [ShapeType.triangleMesh]: noOp,
+  },
+} as const;
+
+export class CollideShapesModule {
+  penetrationDepthModule: PenetrationDepthModule;
+
+  constructor(penetrationDepthModule: PenetrationDepthModule) {
+    this.penetrationDepthModule = penetrationDepthModule;
+  }
+  // collides two shapes that implement the ShapeWithType interface, using the query map
+  // the shapes must be in local space
+  // the isometries must transform the shapes into world space
+  // the results are in world space
+  collideShapes(
+    collector: CollisionCollector,
+    shapeA: Shape,
+    isometryA: Isometry,
+    shapeB: Shape,
+    isometryB: Isometry,
+    settings: CollisionSettings,
+    bodyA: Body | null,
+    bodyB: Body | null,
+    subShapeIdA: number = 0,
+    subShapeIdB: number = 0
+  ): void {
+    const keyA = shapeA.type as keyof typeof collideShapesFns;
+    const keyB = shapeB.type as keyof (typeof collideShapesFns)[keyof typeof collideShapesFns];
+    const collisionFunction = collideShapesFns[keyA][keyB];
+    if (!collisionFunction) {
+      throw new Error(`no collision function found for shapes with type ${keyA} and ${keyB}`);
+    }
+
+    return collisionFunction(
+      this.penetrationDepthModule,
+      collector,
+      // @ts-ignore
+      shapeA,
+      shapeB,
+      isometryA,
+      isometryB,
+      settings,
+      bodyA,
+      bodyB,
+      undefined,
+      subShapeIdA,
+      subShapeIdB
+    );
+  }
+}

package/src/collision/HeightMapCaster.ts

@@ -0,0 +1,38 @@
+import { Vec3 } from "../math/vec3";
+import { Body } from "../physics/Body";
+import { Aabb } from "../shape/Aabb";
+import { HeightMapVisitor } from "../shape/HeightMap";
+import { Ray } from "../shape/Ray";
+import { Triangle } from "../shape/Triangle";
+import { TriangleCaster } from "./TriangleCaster";
+
+export class HeightMapCaster implements HeightMapVisitor {
+  halfExtents: Vec3;
+  ray: Ray;
+  triangleCaster: TriangleCaster;
+  bodyA: Body | null = null;
+  bodyB: Body | null = null;
+
+  constructor(triangleCaster: TriangleCaster) {
+    this.triangleCaster = triangleCaster;
+    this.halfExtents = Vec3.create() as Vec3;
+    this.ray = Ray.create() as Ray;
+  }
+
+  initialize(halfExtents: Vec3, ray: Ray, bodyA: Body | null, bodyB: Body | null): void {
+    this.halfExtents.copy(halfExtents);
+    this.ray.copy(ray);
+    this.bodyA = bodyA;
+    this.bodyB = bodyB;
+  }
+
+  shouldVisitBlock(block: Aabb): boolean {
+    // expand the block aabb by the extents of the shape aabb
+    block.expandByVector(this.halfExtents);
+    return this.ray.intersectsAabb(block);
+  }
+
+  visitTriangle(triangle: Triangle, activeEdges: number, triangleId: number): void {
+    this.triangleCaster.cast(triangle, activeEdges, triangleId, this.bodyA, this.bodyB);
+  }
+}

package/src/collision/HeightMapCollider.ts

@@ -0,0 +1,33 @@
+import { Body } from "../physics/Body";
+import { Aabb } from "../shape/Aabb";
+import { HeightMapVisitor } from "../shape/HeightMap";
+import { Triangle } from "../shape/Triangle";
+import { TriangleCollider } from "./TriangleCollider";
+
+export class HeightMapCollider implements HeightMapVisitor {
+  bodyA: Body | null;
+  bodyB: Body | null;
+  triangleCollider: TriangleCollider;
+  shapeABoundsInBSpace: Aabb;
+
+  constructor() {
+    this.shapeABoundsInBSpace = Aabb.create() as Aabb;
+    this.triangleCollider = new TriangleCollider();
+    this.bodyA = null;
+    this.bodyB = null;
+  }
+
+  initialize(shapeABoundsInBSpace: Aabb, bodyA: Body | null, bodyB: Body | null): void {
+    this.shapeABoundsInBSpace.copy(shapeABoundsInBSpace);
+    this.bodyA = bodyA;
+    this.bodyB = bodyB;
+  }
+
+  shouldVisitBlock(block: Aabb): boolean {
+    return this.shapeABoundsInBSpace.intersectsAabb(block);
+  }
+
+  visitTriangle(triangle: Triangle, activeEdges: number, triangleId: number): void {
+    this.triangleCollider.collide(triangle, activeEdges, triangleId, this.bodyA, this.bodyB);
+  }
+}

package/src/collision/TriangleCaster.ts

@@ -0,0 +1,249 @@
+import { Isometry } from "../math/isometry";
+import { Mat3 } from "../math/mat3";
+import { NumberValue } from "../math/NumberValue";
+import { Vec3 } from "../math/vec3";
+import { Body } from "../physics/Body";
+import { SupportMode } from "../shape/Convex";
+import { ConvexShape } from "../shape/Shape";
+import { Triangle } from "../shape/Triangle";
+import { fixNormal } from "./activeEdge";
+import { CastCollector, CastResult, CastSettings } from "./cast/cast";
+import { ActiveEdgeMode, BackFaceMode } from "./collide/collide";
+import { GjkCastShapeResult, GjkModule } from "./gjk/GjkModule";
+import { PenetrationDepthModule } from "./gjk/PenetrationDepthModule";
+
+const triangleNormal = /*@__PURE__*/ Vec3.create();
+const negatedTriangleNormal = /*@__PURE__*/ Vec3.create();
+const pointA = /*@__PURE__*/ Vec3.create();
+const pointB = /*@__PURE__*/ Vec3.create();
+const penetrationAxis = /*@__PURE__*/ Vec3.create();
+const lambda = /*@__PURE__*/ NumberValue.create();
+const castResult = /*@__PURE__*/ GjkCastShapeResult.create();
+const activeEdgeMovementDirectionLocal = /*@__PURE__*/ Vec3.create();
+const contactNormal = /*@__PURE__*/ Vec3.create();
+const result = /*@__PURE__*/ CastResult.create();
+const rotationMatrixB = /*@__PURE__*/ Mat3.create();
+const penetrationDepthModule = /*@__PURE__*/ new PenetrationDepthModule(new GjkModule());
+const activeEdgeMovementDirection = /*@__PURE__*/ Vec3.create();
+
+export class TriangleCaster {
+  declare penetrationDepthModule: PenetrationDepthModule;
+
+  declare settings: CastSettings;
+  declare collector: CastCollector;
+  declare shapeA: ConvexShape;
+  bodyA: Body | null = null;
+  bodyB: Body | null = null;
+  shapeB: Triangle;
+  negatedOffset: Vec3;
+  offsetIsometryA: Isometry;
+  offsetIsometryB: Isometry;
+  inverseOffsetIsometryB: Isometry;
+  offsetIsometryAToB: Isometry;
+  displacementInB: Vec3;
+  rotateBToWorld: Mat3;
+  rotateWorldToB: Mat3;
+
+  constructor() {
+    this.shapeB = Triangle.create() as Triangle;
+    this.negatedOffset = Vec3.create() as Vec3;
+    this.offsetIsometryA = Isometry.create() as Isometry;
+    this.offsetIsometryB = Isometry.create() as Isometry;
+    this.inverseOffsetIsometryB = Isometry.create() as Isometry;
+    this.offsetIsometryAToB = Isometry.create() as Isometry;
+    this.displacementInB = Vec3.create() as Vec3;
+    this.rotateBToWorld = Mat3.create() as Mat3;
+    this.rotateWorldToB = Mat3.create() as Mat3;
+  }
+
+  initialize(
+    settings: CastSettings,
+    collector: CastCollector,
+    shapeA: ConvexShape,
+    isometryA: Isometry,
+    isometryB: Isometry,
+    displacement: Vec3,
+    offset: Vec3,
+    bodyA: Body | null = null,
+    bodyB: Body | null = null
+  ): void {
+    // so far, everything is in world space (as in, the shapes are in local space, and the isometries transform from their local space to world space, respectively)
+    // but we want to cast in B space, so convert everything to B space in the same style as CastModule.castConvexVsConvexWorld
+    this.settings = settings;
+    this.collector = collector;
+    this.shapeA = shapeA;
+    this.bodyA = bodyA;
+    this.bodyB = bodyB;
+
+    const negatedOffset = this.negatedOffset;
+    const offsetIsometryA = this.offsetIsometryA;
+    const offsetIsometryB = this.offsetIsometryB;
+    const inverseOffsetIsometryB = this.inverseOffsetIsometryB;
+    const offsetIsometryAToB = this.offsetIsometryAToB;
+    const displacementInB = this.displacementInB;
+
+    // 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 the rotation matrix of B
+    this.rotateBToWorld.fromMat4(isometryB.matrix);
+
+    // get the rotation matrix of world to B
+    this.rotateWorldToB.invertMat3(this.rotateBToWorld);
+
+    // get displacement in the space of B
+    inverseOffsetIsometryB.matrix.multiply3x3(displacementInB, displacement);
+  }
+
+  cast(triangle: Triangle, activeEdges: number, triangleId: number, bodyA: Body | null, bodyB: Body | null): void {
+    // cast in B space, return in world space
+    const ioCollector = this.collector;
+    const inShapeA = this.shapeA;
+    const inShapeB = this.shapeB;
+    const isometryAToB = this.offsetIsometryAToB;
+    const isometryB = this.offsetIsometryB;
+    const inScaleA = 1;
+    const inScaleB = 1;
+    const displacementInB = this.displacementInB;
+    const inShapeCastSettings = this.settings;
+    const rotateBToWorld = this.rotateBToWorld;
+    const rotateWorldToB = this.rotateWorldToB;
+
+    // get shape B in B space
+    inShapeB.copy(triangle);
+
+    // TODO: scale the shapes
+
+    // get the triangle normal in B space
+
+    inShapeB.computeNormal(triangleNormal);
+    negatedTriangleNormal.negateVector(triangleNormal);
+
+    // TODO: handle backface mode
+    const back_facing = triangleNormal.dot(displacementInB) > 0;
+    if (inShapeCastSettings.backFaceModeTriangles === BackFaceMode.IgnoreBackFaces && back_facing) {
+      ioCollector.addMiss();
+      return;
+    }
+
+    // TODO: handle bounds casting
+
+    // get the support shapes
+    const supportMode = inShapeCastSettings.useShrunkenShapeAndConvexRadius
+      ? SupportMode.ExcludeConvexRadius
+      : SupportMode.IncludeConvexRadius;
+    const supportA = inShapeA.computeSupportShape(supportMode, inScaleA);
+    const supportB = inShapeB.computeSupportShape(supportMode, inScaleB);
+
+    // get the convex radii
+    const convexRadiusA = supportA.getConvexRadius();
+    const convexRadiusB = supportB.getConvexRadius();
+
+    // init pass-through results
+
+    // init the gjk cast shape result
+
+    castResult.lambda = ioCollector.getEarlyOutFraction();
+    penetrationDepthModule.castShape(
+      castResult,
+      isometryAToB,
+      displacementInB,
+      inShapeCastSettings.collisionTolerance,
+      inShapeCastSettings.penetrationTolerance,
+      supportA,
+      supportB,
+      convexRadiusA,
+      convexRadiusB,
+      inShapeCastSettings.returnDeepestPoint
+    );
+
+    // exit if no hit found
+    if (castResult.isHitFound === false) {
+      this.collector.addMiss();
+      return;
+    }
+
+    // update pass-through results
+    pointA.copy(castResult.pointA);
+    pointB.copy(castResult.pointB);
+    penetrationAxis.copy(castResult.separatingAxis);
+    // vec3.normalize(penetrationAxis, castResult.separatingAxis);
+    lambda.value = castResult.lambda;
+
+    // check active edge mode
+    if (inShapeCastSettings.activeEdgeMode === ActiveEdgeMode.CollideOnlyWithActive && activeEdges !== 0b111) {
+      // debugger;
+      // convert the active edge velocity hint to local space
+
+      activeEdgeMovementDirection.x = inShapeCastSettings.activeEdgeMovementDirectionX;
+      activeEdgeMovementDirection.y = inShapeCastSettings.activeEdgeMovementDirectionY;
+      activeEdgeMovementDirection.z = inShapeCastSettings.activeEdgeMovementDirectionZ;
+
+      activeEdgeMovementDirectionLocal.transformVectorFromMat3(activeEdgeMovementDirection, rotateWorldToB);
+
+      // update the contact normal to account for active edges
+      // note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+
+      fixNormal(
+        contactNormal,
+        inShapeB.a,
+        inShapeB.b,
+        inShapeB.c,
+        back_facing ? triangleNormal : negatedTriangleNormal,
+        activeEdges,
+        pointB,
+        penetrationAxis,
+        activeEdgeMovementDirectionLocal
+      );
+
+      // update the penetration axis
+      penetrationAxis.copy(contactNormal);
+      // vec3.negate(penetrationAxis, contactNormal);
+    }
+
+    // convert to world space
+    pointA.transformVectorFromMat4(pointA, isometryB.matrix);
+    pointB.transformVectorFromMat4(pointB, isometryB.matrix);
+    penetrationAxis.transformVectorFromMat3(penetrationAxis, rotateBToWorld);
+
+    // create the result
+
+    result.bodyA = bodyA;
+    result.bodyB = bodyB;
+    result.subShapeIdA = 0;
+    result.subShapeIdB = triangleId;
+    result.fraction = lambda.value;
+    result.isBackFaceHit = false;
+    result.contactPointA.copy(pointA);
+    result.contactPointB.copy(pointB);
+    result.normalA.copy(penetrationAxis);
+    result.normalB.negateVector(penetrationAxis);
+    result.penetration = pointA.distance(pointB);
+
+    // early out if this hit is deeper than the collector's early out value
+    if (result.fraction === 0 && -result.penetration >= ioCollector.getEarlyOutFraction()) {
+      ioCollector.addMiss(); // TODO: should we classify as a miss?
+      return;
+    }
+
+    // set surface normals on result
+    result.surfaceNormalA.zero();
+
+    // surface normal B should be the triangle's normal in world space
+
+    rotationMatrixB.fromMat4(isometryB.matrix);
+    result.surfaceNormalB.transformVectorFromMat3(triangleNormal, rotationMatrixB);
+
+    // TODO: handle faces
+
+    // add the result to the collector
+    // ioCollector.earlyOutFraction = result.fraction;
+    ioCollector.addHit(result);
+  }
+}