@perplexdotgg/bounce 1.0.0

package/src/collision/activeEdge.ts

@@ -0,0 +1,146 @@
+// activeEdges (uint8bit): 0 = edge AB active, bit 1 = edge BC active, bit 2 = edge CA active
+
+import { degreesToRadians } from "../math/scalar";
+import { Vec3 } from "../math/vec3";
+import { BarycentricCoordinatesResult } from "./closestPoints/closestPoints";
+import { computeBarycentricCoordinates3d } from "./closestPoints/computeBarycentricCoordinates3d";
+
+const cos1degree = Math.cos(degreesToRadians(1));
+const cos179degrees = Math.cos(degreesToRadians(179));
+const epsilon = 1e-4;
+const oneMinusEpsilon = 1 - epsilon;
+
+const result = /*@__PURE__*/ BarycentricCoordinatesResult.create();
+const crossNormal1Normal2 = /*@__PURE__*/ Vec3.create();
+const pa = /*@__PURE__*/ Vec3.create();
+const pb = /*@__PURE__*/ Vec3.create();
+const pc = /*@__PURE__*/ Vec3.create();
+
+export function isEdgeActive(
+  inNormal1: Vec3,
+  inNormal2: Vec3,
+  inEdgeDirection: Vec3,
+  inCosThresholdAngle: number
+): boolean {
+  // If normals are opposite the edges are active (the triangles are back to back)
+  const cosAngleBetweenNormals = inNormal1.dot(inNormal2);
+
+  // case: triangles are back to back -> edge is active
+  if (cosAngleBetweenNormals < cos179degrees) {
+    return true;
+  }
+
+  crossNormal1Normal2.crossVectors(inNormal1, inNormal2);
+
+  // case: concave edge -> edge is inactive
+  if (crossNormal1Normal2.dot(inEdgeDirection) < 0) {
+    return false;
+  }
+
+  // case: convex edge -> edge is active when angle between normals is bigger than threshold
+  return cosAngleBetweenNormals < inCosThresholdAngle;
+}
+
+export function fixNormal(
+  out: Vec3,
+  inV0: Vec3,
+  inV1: Vec3,
+  inV2: Vec3,
+  inTriangleNormal: Vec3,
+  inActiveEdges: number,
+  inPoint: Vec3,
+  inNormal: Vec3,
+  inMovementDirection: Vec3
+): void {
+  // #v-ifdef DEV
+  // TODO: assert that not all edges are active, else we already have the correct normal
+  if (inActiveEdges === 0b111) {
+    throw new Error("All edges are active, no need to fix normal");
+  }
+  // #v-endif
+
+  // If inNormal would affect movement less than inTriangleNormal use inNormal
+  // This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal)
+  // and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal).
+  // To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal.
+  const normal_length = inNormal.length();
+  const triangle_normal_length = inTriangleNormal.length();
+  if (
+    inMovementDirection.dot(inNormal) * triangle_normal_length <
+    inMovementDirection.dot(inTriangleNormal) * normal_length
+  ) {
+    out.copy(inNormal);
+    return;
+  }
+
+  // Check: None of the edges are active, we need to use the triangle normal
+  if (inActiveEdges === 0) {
+    out.copy(inTriangleNormal);
+    return;
+  }
+
+  // Some edges are active.
+  // If normal is parallel to the triangle normal we don't need to check the active edges.
+  if (inTriangleNormal.dot(inNormal) > cos1degree * normal_length * triangle_normal_length) {
+    out.copy(inNormal);
+    return;
+  }
+
+  let collidingEdge: number;
+
+  // Test where the contact point is in the triangle
+  result.u;
+  result.v;
+  result.w;
+
+  pa.subtractVectors(inV0, inPoint);
+  pb.subtractVectors(inV1, inPoint);
+  pc.subtractVectors(inV2, inPoint);
+
+  computeBarycentricCoordinates3d(result, pa, pb, pc);
+
+  // Colliding with v0, edge 0 or 2 needs to be active
+  if (result.u > oneMinusEpsilon) {
+    collidingEdge = 0b101;
+  }
+
+  // Colliding with v1, edge 0 or 1 needs to be active
+  else if (result.v > oneMinusEpsilon) {
+    collidingEdge = 0b011;
+  }
+
+  // Colliding with v2, edge 1 or 2 needs to be active
+  else if (result.w > oneMinusEpsilon) {
+    collidingEdge = 0b110;
+  }
+
+  // Colliding with edge v1, v2, edge 1 needs to be active
+  else if (result.u < epsilon) {
+    collidingEdge = 0b010;
+  }
+
+  // Colliding with edge v0, v2, edge 2 needs to be active
+  else if (result.v < epsilon) {
+    collidingEdge = 0b100;
+  }
+
+  // Colliding with edge v0, v1, edge 0 needs to be active
+  else if (result.w < epsilon) {
+    collidingEdge = 0b001;
+  }
+
+  // Interior hit
+  else {
+    out.copy(inTriangleNormal);
+    return;
+  }
+
+  // If this edge is active, use the provided normal
+  if ((inActiveEdges & collidingEdge) !== 0) {
+    out.copy(inNormal);
+    return;
+  }
+
+  // if this edge is not active, use the triangle normal
+  out.copy(inTriangleNormal);
+}

package/src/collision/cast/cast.ts

@@ -0,0 +1,139 @@
+import {
+  BooleanType,
+  createClass,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { Vec3 } from "../../math/vec3";
+import { Face } from "../../physics/manifold/Face";
+import { BackFaceMode, CollisionSettings, createDefaultCollisionSettings } from "../collide/collide";
+import { Body } from "../../physics/Body";
+
+export const enum CastStatus {
+  NotColliding,
+  Colliding,
+  Indeterminate,
+}
+
+const castResultProps = {
+  status: NumberType(0),
+  hasContact: BooleanType(false),
+  penetration: NumberType(0.0),
+  contactPointA: MonomorphType(Vec3),
+  contactPointB: MonomorphType(Vec3),
+  normalA: MonomorphType(Vec3),
+  normalB: MonomorphType(Vec3),
+  momentArmA: MonomorphType(Vec3),
+  momentArmB: MonomorphType(Vec3),
+  surfaceNormalA: MonomorphType(Vec3),
+  surfaceNormalB: MonomorphType(Vec3),
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  subShapeIdA: NumberType(0),
+  subShapeIdB: NumberType(0),
+  isBackFace: BooleanType(false),
+  faceA: MonomorphType(Face),
+  faceB: MonomorphType(Face),
+  fraction: NumberType(0.0),
+  isBackFaceHit: BooleanType(false),
+} as const satisfies PropertyDefinitionMap;
+
+export class CastResult extends createClass<CastResult, typeof castResultProps>(castResultProps) {
+  reset() {
+    this.status = CastStatus.Indeterminate;
+    this.hasContact = false;
+    this.penetration = 0;
+    this.bodyA = null;
+    this.bodyB = null;
+    this.subShapeIdA = 0;
+    this.subShapeIdB = 0;
+    this.isBackFace = false;
+    this.faceA.clear();
+    this.faceB.clear();
+  }
+
+  /**
+   * swaps the A and B data
+   */
+  swap() {
+    this.contactPointA.swap(this.contactPointB);
+    this.normalA.swap(this.normalB);
+    this.momentArmA.swap(this.momentArmB);
+    this.surfaceNormalA.swap(this.surfaceNormalB);
+
+    const tempBody = this.bodyA;
+    this.bodyA = this.bodyB;
+    this.bodyB = tempBody;
+
+    const tempSubShapeId = this.subShapeIdA;
+    this.subShapeIdA = this.subShapeIdB;
+    this.subShapeIdB = tempSubShapeId;
+
+    this.faceA.swap(this.faceB);
+  }
+
+  getEarlyOutFraction(): number {
+    return this.fraction > 0.0 ? this.fraction : -this.penetration;
+  }
+}
+
+export class CastCollector {
+  static initialEarlyOutFraction: number = 1.0 + 1e-4;
+  static shouldEarlyOutFraction: number = -Infinity;
+  earlyOutFraction: number = CastCollector.initialEarlyOutFraction;
+  body2: Body | null = null;
+
+  getEarlyOutFraction(): number {
+    return this.earlyOutFraction;
+  }
+
+  getPositiveEarlyOutFraction(): number {
+    return Math.max(Number.MIN_VALUE, this.earlyOutFraction);
+  }
+
+  reset() {
+    this.earlyOutFraction = CastCollector.initialEarlyOutFraction;
+    this.body2 = null;
+
+    return this;
+  }
+
+  updateEarlyOutFraction(inFraction: number): void {
+    if (inFraction <= this.earlyOutFraction) {
+      this.earlyOutFraction = inFraction;
+    }
+  }
+
+  addHit(result: CastResult): void {}
+
+  addMiss(): void {}
+}
+
+export interface CastSettings extends CollisionSettings {
+  backFaceModeConvex: BackFaceMode;
+  backFaceModeTriangles: BackFaceMode;
+  useShrunkenShapeAndConvexRadius: boolean;
+  returnDeepestPoint: boolean;
+}
+export const enum QueryPrecision {
+  approximate,
+  precise,
+  preciseWithContacts,
+}
+export interface RayCastSettings {
+  precision?: QueryPrecision;
+  returnClosestOnly?: boolean;
+}
+
+export function createDefaultCastSettings() {
+  return {
+    ...createDefaultCollisionSettings(),
+    backFaceModeConvex: BackFaceMode.IgnoreBackFaces,
+    backFaceModeTriangles: BackFaceMode.IgnoreBackFaces,
+    useShrunkenShapeAndConvexRadius: false,
+    returnDeepestPoint: false,
+  } satisfies CastSettings;
+}

package/src/collision/cast/castCompoundVsCompound.ts

@@ -0,0 +1,59 @@
+// import { Isometry } from "../../math/isometry";
+// import { Vec3 } from "../../math/vec3";
+// import { CompoundShape } from "../../shape/CompoundShape";
+// import { ConvexWithcomputeSupportShape } from "../../shape/Convex";
+// import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+// import { CastCollector, CastSettings } from "./cast";
+// import { castConvexVsConvex } from "./castConvexVsConvex";
+
+// 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 castCompoundVsCompound(
+//   penetrationDepthModule: PenetrationDepthModule,
+//   ioCollector: CastCollector,
+//   inShapeA: CompoundShape,
+//   inShapeB: CompoundShape,
+//   isometryA: Isometry,
+//   isometryB: Isometry,
+//   inScaleA: number,
+//   inScaleB: number,
+//   displacement: Vec3,
+//   offset: Vec3,
+//   inSettings: CastSettings,
+//   preferredNormalBDirection?: Vec3,
+//   bodyIdA: number = 0,
+//   bodyIdB: number = 0
+// ): 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.invertMat4(offsetIsometryB.matrix);
+//   offsetIsometryAToB.matrix.multiplyMatrices(inverseOffsetIsometryB.matrix, offsetIsometryA.matrix);
+
+//   // get displacement in the space of B
+//   inverseOffsetIsometryB.matrix.multiply3x3(displacementInB, displacement);
+
+//   castConvexVsConvex(
+//     penetrationDepthModule,
+//     ioCollector,
+//     inShapeA,
+//     inShapeB,
+//     offsetIsometryAToB,
+//     offsetIsometryB,
+//     inScaleA,
+//     inScaleB,
+//     displacementInB,
+//     inSettings,
+//     preferredNormalBDirection,
+//     bodyIdA,
+//     bodyIdB
+//   );
+// }

package/src/collision/cast/castCompoundVsConvex.ts

@@ -0,0 +1,116 @@
+import { BasicTransform } from "../../math/BasicTransform";
+import { Isometry } from "../../math/isometry";
+import { Vec3 } from "../../math/vec3";
+import { Body } from "../../physics/Body";
+import { CompoundShape } from "../../shape/CompoundShape";
+import { ConvexWithcomputeSupportShape } from "../../shape/Convex";
+import { ConvexShape } from "../../shape/Shape";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastSettings } from "./cast";
+import { castConvexVsConvexLocal } from "./castConvexVsConvex";
+
+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 castCompoundVsConvex(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: CompoundShape,
+  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);
+
+  castCompoundVsConvexLocal(
+    penetrationDepthModule,
+    ioCollector,
+    inShapeA,
+    inShapeB,
+    offsetIsometryAToB,
+    offsetIsometryB,
+    inScaleA,
+    inScaleB,
+    displacementInB,
+    inSettings,
+    preferredNormalBDirection,
+    bodyA,
+    bodyB
+  );
+}
+
+const transformSubShapeToA = /*@__PURE__*/ Isometry.create();
+const transformSubShapeToB = /*@__PURE__*/ Isometry.create();
+
+export function castCompoundVsConvexLocal(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: CompoundShape,
+  inShapeB: ConvexWithcomputeSupportShape,
+  isometryAToB: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacementInB: Vec3,
+  inShapeCastSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+): void {
+  // debugger;
+  // for (let i = 0; i < inShapeA.shapeIds.itemMaxCount; i++) {
+  let index = 0;
+  for (const shapeObj of inShapeA.shapes) {
+    const shape = shapeObj.shape as ConvexShape;
+    const transform = shapeObj.transform as BasicTransform;
+    const translation = transform.position;
+    const rotation = transform.rotation;
+
+    // set the isometries
+    transformSubShapeToA.fromRotationAndTranslation(rotation, translation);
+    transformSubShapeToB.matrix.multiplyMatrices(isometryAToB.matrix, transformSubShapeToA.matrix);
+
+    // cast the shapes
+    castConvexVsConvexLocal(
+      penetrationDepthModule,
+      ioCollector,
+      shape,
+      inShapeB,
+      transformSubShapeToB,
+      isometryB,
+      inScaleA,
+      inScaleB,
+      displacementInB,
+      inShapeCastSettings,
+      preferredNormalBDirection,
+      bodyA,
+      bodyB,
+      index,
+      0
+    );
+
+    index++;
+  }
+}

package/src/collision/cast/castConvexVsCompound.ts

@@ -0,0 +1,123 @@
+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 } from "../../shape/Convex";
+import { PenetrationDepthModule } from "../gjk/PenetrationDepthModule";
+import { CastCollector, CastSettings } from "./cast";
+import { castConvexVsConvexLocal } from "./castConvexVsConvex";
+
+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 castConvexVsCompound(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexWithcomputeSupportShape,
+  inShapeB: CompoundShape,
+  isometryA: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacement: Vec3,
+  offset: Vec3,
+  inSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+) {
+  // 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);
+
+  castConvexVsCompoundLocal(
+    penetrationDepthModule,
+    ioCollector,
+    inShapeA,
+    inShapeB,
+    offsetIsometryAToB,
+    offsetIsometryB,
+    inScaleA,
+    inScaleB,
+    displacementInB,
+    inSettings,
+    preferredNormalBDirection,
+    bodyA,
+    bodyB
+  );
+}
+
+const transformSubShapeToB = /*@__PURE__*/ Isometry.create();
+const transformBToSubShape = /*@__PURE__*/ Isometry.create();
+const transformAToSubShape = /*@__PURE__*/ Isometry.create();
+const transformSubShapeToWorld = /*@__PURE__*/ Isometry.create();
+const displacementInSubShape = /*@__PURE__*/ Vec3.create();
+
+export function castConvexVsCompoundLocal(
+  penetrationDepthModule: PenetrationDepthModule,
+  ioCollector: CastCollector,
+  inShapeA: ConvexWithcomputeSupportShape,
+  inShapeB: CompoundShape,
+  isometryAToB: Isometry,
+  isometryB: Isometry,
+  inScaleA: number,
+  inScaleB: number,
+  displacementInB: Vec3,
+  inShapeCastSettings: CastSettings,
+  preferredNormalBDirection?: Vec3,
+  bodyA: Body | null = null,
+  bodyB: Body | null = null
+): void {
+  let index = 0;
+  for (const shapeObj of inShapeB.shapes) {
+    const shape = shapeObj.shape as ConvexShape;
+    const transform = shapeObj.transform as BasicTransform;
+    const translation = transform.position;
+    const rotation = transform.rotation;
+
+    // set the isometries
+    transformSubShapeToB.fromRotationAndTranslation(rotation, translation);
+    transformBToSubShape.fromInverseRotationAndTranslation(rotation, translation);
+    transformAToSubShape.matrix.multiplyMatrices(transformBToSubShape.matrix, isometryAToB.matrix);
+
+    // set the displacement
+    transformBToSubShape.matrix.multiply3x3(displacementInSubShape, displacementInB);
+
+    // subshape to world transform to transform results back to world space
+    transformSubShapeToWorld.matrix.multiplyMatrices(isometryB.matrix, transformSubShapeToB.matrix);
+
+    // cast the shapes
+    castConvexVsConvexLocal(
+      penetrationDepthModule,
+      ioCollector,
+      inShapeA,
+      shape,
+      transformAToSubShape,
+      transformSubShapeToWorld,
+      inScaleA,
+      inScaleB,
+      displacementInSubShape,
+      inShapeCastSettings,
+      preferredNormalBDirection,
+      bodyA,
+      bodyB,
+      0,
+      index
+    );
+
+    index++;
+  }
+}