@perplexdotgg/bounce 1.0.0

package/src/collision/imp/findContactImp.ts

@@ -0,0 +1,196 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+import { MprResult, MprStatus } from "../mpr/mpr";
+import { MinkowskiDifference } from "./MinkowskiDifference";
+import { finalizeImpResult } from "./finalizeImpResult";
+import { ImpResult, ImpStatus } from "./imp";
+import { incrementalMinimumDistanceExploreDirection } from "./incrementalMinimumDistanceExploreDirection";
+
+const support = /*@__PURE__*/ MinkowskiDifference.create();
+const d = /*@__PURE__*/ Vec3.create();
+const v1 = /*@__PURE__*/ Vec3.create();
+const v2 = /*@__PURE__*/ Vec3.create();
+const v3 = /*@__PURE__*/ Vec3.create();
+const v1_dir_in_support = /*@__PURE__*/ Vec3.create();
+const v2_dir_in_support = /*@__PURE__*/ Vec3.create();
+const v3_dir_in_support = /*@__PURE__*/ Vec3.create();
+const new_d = /*@__PURE__*/ Vec3.create();
+const prev_direction = /*@__PURE__*/ Vec3.create();
+const direction_difference = /*@__PURE__*/ Vec3.create();
+const prev_bounds: [number, number] = [0, 0];
+const bounds: [number, number] = [0, 0];
+const mprResult = /*@__PURE__*/ new MprResult();
+
+export function findContactImp(
+  refine_data: ImpResult,
+  shapeA: SupportShape,
+  shapeB: SupportShape,
+  init_direction: Vec3,
+  max_iteration: number,
+  tolerance: number,
+  return_on_subroutine_converge: boolean
+): void {
+  // resetMprResult(mprResult);
+  d.copy(init_direction);
+  // #v-ifdef DEV
+  assert(d.isUnitVector(), "d must be a unit vector");
+  // #v-endif
+
+  // The main loop
+  prev_direction.copy(init_direction);
+  prev_bounds[0] = -1;
+  prev_bounds[1] = -1;
+  for (let outer_iteration = 0; outer_iteration < max_iteration; outer_iteration++) {
+    // Invoke subroutine
+    bounds[0] = 0;
+    bounds[1] = 0;
+    const portal_valid = outer_iteration >= 1;
+    incrementalMinimumDistanceExploreDirection(
+      mprResult,
+      shapeA,
+      shapeB,
+      d,
+      v1,
+      v2,
+      v3,
+      v1_dir_in_support,
+      v2_dir_in_support,
+      v3_dir_in_support,
+      bounds,
+      new_d,
+      portal_valid,
+      max_iteration,
+      tolerance
+    );
+
+    // Update the output
+    switch (mprResult.status) {
+      case MprStatus.NewDirection: {
+        d.copy(new_d);
+        break;
+      }
+
+      case MprStatus.SubroutineConverge: {
+        // directly return without further checking
+        if (return_on_subroutine_converge) {
+          finalizeImpResult(
+            refine_data,
+            shapeA,
+            shapeB,
+            d,
+            v1,
+            v2,
+            v3,
+            v1_dir_in_support,
+            v2_dir_in_support,
+            v3_dir_in_support,
+            new_d,
+            bounds
+          );
+          refine_data.status = ImpStatus.OK;
+          return;
+        }
+
+        // Prev-data is valid, use that to check the convergence
+        if (outer_iteration >= 1) {
+          // #v-ifdef DEV
+          assert(prev_bounds[0] >= 0, "prev_bounds[0] must be valid");
+          assert(prev_bounds[1] >= 0, "prev_bounds[1] must be valid");
+          // #v-endif
+          if (Math.abs(bounds[1] - prev_bounds[1]) <= tolerance) {
+            // Done as not much improvement
+            finalizeImpResult(
+              refine_data,
+              shapeA,
+              shapeB,
+              d,
+              v1,
+              v2,
+              v3,
+              v1_dir_in_support,
+              v2_dir_in_support,
+              v3_dir_in_support,
+              new_d,
+              bounds
+            );
+            refine_data.status = ImpStatus.OK;
+            return;
+          }
+        }
+
+        // Check direction
+        const check_direction_termination = true;
+        const direction_tolerance = 1e-3;
+        direction_difference.subtractVectors(new_d, prev_direction);
+        if (check_direction_termination && direction_difference.length() < direction_tolerance) {
+          finalizeImpResult(
+            refine_data,
+            shapeA,
+            shapeB,
+            d,
+            v1,
+            v2,
+            v3,
+            v1_dir_in_support,
+            v2_dir_in_support,
+            v3_dir_in_support,
+            new_d,
+            bounds
+          );
+          refine_data.status = ImpStatus.OK;
+          return;
+        }
+
+        // Move to next, as the subroutine converge
+        // The portal can be rather small here
+        // portal_valid = false;
+        d.copy(new_d);
+        break;
+      }
+
+      case MprStatus.Degenerated: {
+        // Done on degenerate
+        refine_data.minimum_penetration = bounds[1];
+        refine_data.penetration_direction.copy(new_d);
+        support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, new_d);
+        refine_data.worldContactPointA.copy(support.pointA);
+        refine_data.worldContactPointB.copy(support.pointB);
+        refine_data.status = ImpStatus.OK;
+        return;
+      }
+
+      case MprStatus.FailedNoIntersect: {
+        refine_data.status = ImpStatus.FailedNoIntersect;
+        return;
+      }
+
+      default:
+        refine_data.status = ImpStatus.Failed;
+        return;
+    }
+
+    // Move to next iteration, book keeping
+    prev_direction.copy(new_d);
+    prev_bounds[0] = bounds[0];
+    prev_bounds[1] = bounds[1];
+  }
+
+  // Too many iterations
+  finalizeImpResult(
+    refine_data,
+    shapeA,
+    shapeB,
+    d,
+    v1,
+    v2,
+    v3,
+    v1_dir_in_support,
+    v2_dir_in_support,
+    v3_dir_in_support,
+    prev_direction,
+    prev_bounds
+  );
+  refine_data.status = ImpStatus.IterationLimit;
+  return;
+}

package/src/collision/imp/imp.ts

@@ -0,0 +1,28 @@
+import { createClass, MonomorphType, NumberType, PropertyDefinition, PropertyDefinitionMap } from "monomorph";
+import { Vec3 } from "../../math/vec3";
+
+export const enum ImpStatus {
+  OK,
+  Failed,
+  FailedNoIntersect,
+  DirectedPenetrationFailed,
+  IterationLimit,
+}
+
+const impResultProps = {
+  status: NumberType(ImpStatus.Failed) as PropertyDefinition<ImpStatus, true>,
+  minimum_penetration: NumberType(0.0),
+  penetration_direction: MonomorphType(Vec3),
+  worldContactPointA: MonomorphType(Vec3),
+  worldContactPointB: MonomorphType(Vec3),
+} as const satisfies PropertyDefinitionMap;
+
+export class ImpResult extends createClass<ImpResult, typeof impResultProps>(impResultProps) {
+  reset() {
+    this.status = ImpStatus.Failed;
+    this.minimum_penetration = 0;
+    this.penetration_direction.zero();
+    this.worldContactPointA.zero();
+    this.worldContactPointB.zero();
+  }
+}

package/src/collision/imp/incrementalMinimumDistanceExploreDirection.ts

@@ -0,0 +1,207 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+import { findPortal } from "../mpr/findPortal";
+import { FindPortalResult, FindPortalStatus, MprResult, MprStatus } from "../mpr/mpr";
+import { updatePortal } from "../mpr/updatePortal";
+import { MinkowskiDifference } from "./MinkowskiDifference";
+import { computeExploredDistanceLowerUpperBound } from "./computeExploredDistanceLowerUpperBound";
+
+const support = /*@__PURE__*/ MinkowskiDifference.create();
+const d = /*@__PURE__*/ Vec3.create();
+const v0 = /*@__PURE__*/ Vec3.create();
+const v0_scaled = /*@__PURE__*/ Vec3.create();
+const init_support_dir = /*@__PURE__*/ Vec3.create();
+const init_support = /*@__PURE__*/ Vec3.create();
+const v123_normal = /*@__PURE__*/ Vec3.create();
+const v4 = /*@__PURE__*/ Vec3.create();
+const v12 = /*@__PURE__*/ Vec3.create();
+const v13 = /*@__PURE__*/ Vec3.create();
+
+const findPortalResult = /*@__PURE__*/ new FindPortalResult();
+
+export function incrementalMinimumDistanceExploreDirection(
+  result: MprResult,
+  shapeA: SupportShape,
+  shapeB: SupportShape,
+  direction_to_explore: Vec3,
+  v1: Vec3,
+  v2: Vec3,
+  v3: Vec3,
+  v1_dir_in_support: Vec3,
+  v2_dir_in_support: Vec3,
+  v3_dir_in_support: Vec3,
+  bounds: [number, number],
+  new_direction: Vec3,
+  v123_valid: boolean,
+  max_iterations: number,
+  tolerance: number
+): void {
+  // resetFindPortalResult(findPortalResult);
+
+  d.normalizeVector(direction_to_explore);
+  v0.negateVector(d);
+
+  // Compute the upper bound
+  init_support_dir.copy(d);
+  support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, init_support_dir);
+  init_support.copy(support.point);
+
+  // No intersect case
+  if (support.isOriginBeyondSupportPoint) {
+    result.status = MprStatus.FailedNoIntersect;
+    return;
+  }
+
+  // Do we need to re-init v123
+  const reinit_v123 = !v123_valid;
+
+  // Init v1, v2 and v3
+  if (reinit_v123) {
+    v1_dir_in_support.copy(d);
+    v1.copy(init_support);
+
+    v2_dir_in_support.crossVectors(v0, v1);
+    v2_dir_in_support.normalizeVector(v2_dir_in_support);
+    // o_to_v0 and o_to_v1 can be co-linear, check it
+    // Note that v0 MUST have norm one
+    if (v2_dir_in_support.absNorm() <= v1.absNorm() * tolerance) {
+      // Refer to the note in RunIntersect
+      bounds[0] = v1.dot(d);
+      bounds[1] = bounds[0];
+      new_direction.copy(d);
+      result.status = MprStatus.Degenerated;
+      return;
+    }
+
+    support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v2_dir_in_support);
+    v2.copy(support.point);
+    if (support.isOriginBeyondSupportPoint) {
+      result.status = MprStatus.FailedNoIntersect;
+      return;
+    }
+
+    // it is better to form portal faces to be oriented "outside" origin
+    // Here O must be an interior point in penetration query
+    v3_dir_in_support.crossVectors(v1, v2);
+    v3_dir_in_support.normalizeVector(v3_dir_in_support);
+    if (v3_dir_in_support.dot(v0) < 0) {
+      // swap v1/v2
+      v1.swap(v2);
+      v1_dir_in_support.swap(v2_dir_in_support);
+      v3_dir_in_support.negateVector(v3_dir_in_support);
+    }
+
+    support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v3_dir_in_support);
+    v3.copy(support.point);
+    if (support.isOriginBeyondSupportPoint) {
+      result.status = MprStatus.FailedNoIntersect;
+      return;
+    }
+  }
+
+  // Scale the v0 to the max of v1/v2/v3
+  // #v-ifdef DEV
+  assert(v0.isUnitVector(), "v0 must be a unit vector");
+  // #v-endif
+  v0_scaled.copy(v0);
+  const v1_squared_norm = v1.squaredLength();
+  const v2_squared_norm = v2.squaredLength();
+  const v3_squared_norm = v3.squaredLength();
+  const max_squared_norm = Math.max(v1_squared_norm, v2_squared_norm, v3_squared_norm);
+  v0_scaled.scaleVector(v0_scaled, Math.sqrt(max_squared_norm));
+
+  // the loop to find the portal
+  findPortal(
+    findPortalResult,
+    shapeA,
+    shapeB,
+    v0_scaled,
+    v1,
+    v2,
+    v3,
+    v1_dir_in_support,
+    v2_dir_in_support,
+    v3_dir_in_support,
+    max_iterations
+  );
+
+  switch (findPortalResult.status) {
+    case FindPortalStatus.IterationLimit: {
+      result.status = MprStatus.Failed;
+      return;
+    }
+
+    case FindPortalStatus.DetectSeperated: {
+      result.status = MprStatus.FailedNoIntersect;
+      return;
+    }
+
+    default: {
+      // #v-ifdef DEV
+      assert(findPortalResult.status === FindPortalStatus.PortalFound, "Expecting portal found");
+      // #v-endif
+    }
+  }
+
+  // portal refinement
+  let portal_refinement_iteration = 0;
+  while (portal_refinement_iteration < max_iterations) {
+    // Update iteration data
+    portal_refinement_iteration++;
+
+    // Compute the normal
+    // The v123_normal must be oriented in the same side with O
+    v12.subtractVectors(v2, v1);
+    v13.subtractVectors(v3, v1);
+    v123_normal.crossVectors(v12, v13);
+    v123_normal.normalizeVector(v123_normal);
+    if (v123_normal.dot(d) < 0) {
+      v2.swap(v3);
+      v2_dir_in_support.swap(v3_dir_in_support);
+      v123_normal.negateVector(v123_normal);
+    }
+
+    // A new point v4 on that direction
+    // v123_normal would be normalized in computeShapeSupport
+    support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, v123_normal);
+    v4.copy(support.point);
+    const distance_ub_on_new_supporting_plane = v4.dot(v123_normal);
+    // #v-ifdef DEV
+    assert(v123_normal.isUnitVector(), "Expecting v123_normal to be unit vector");
+    // #v-endif
+    if (support.isOriginBeyondSupportPoint) {
+      result.status = MprStatus.FailedNoIntersect;
+      return;
+    }
+
+    // Compute some distance
+    const v1_dot_v123_normal = v1.dot(v123_normal);
+    const v4_dot_v123_normal = v4.dot(v123_normal);
+    const v1_v4_distance_on_n123 = v4_dot_v123_normal - v1_dot_v123_normal;
+    computeExploredDistanceLowerUpperBound(bounds, d, v1, v2, v3, v123_normal, v1_dot_v123_normal, v4_dot_v123_normal);
+
+    // compute the shortcut
+    if (bounds[0] > distance_ub_on_new_supporting_plane + tolerance) {
+      new_direction.copy(v123_normal);
+      result.status = MprStatus.NewDirection;
+      return;
+    }
+
+    // Separation plane very close to the new (candidate) portal
+    // Note that v123_normal is normalized
+    // We must should stop the subroutine
+    if (Math.abs(v1_v4_distance_on_n123) <= tolerance) {
+      new_direction.copy(v123_normal);
+      result.status = MprStatus.SubroutineConverge;
+      return;
+    }
+
+    // update the portal
+    updatePortal(v0_scaled, v4, v123_normal, v1, v2, v3, v1_dir_in_support, v2_dir_in_support, v3_dir_in_support);
+  }
+
+  // Failure
+  result.status = MprStatus.Failed;
+  return;
+}

package/src/collision/mpr/findPortal.ts

@@ -0,0 +1,152 @@
+import { assert } from "../../helpers";
+import { Vec3 } from "../../math/vec3";
+import { SupportShape } from "../../shape/Convex";
+import { MinkowskiDifference } from "../imp/MinkowskiDifference";
+import { FindPortalResult, FindPortalStatus } from "./mpr";
+
+const dot_eps_ratio = Number.EPSILON; // TODO: this may not be a 1-to-1 substitute for the c++ code: `static constexpr T dot_eps_ratio = std::numeric_limits<T>::epsilon();`
+
+const support = /*@__PURE__*/ MinkowskiDifference.create();
+const v0_to_v1 = /*@__PURE__*/ Vec3.create();
+const v0_to_v2 = /*@__PURE__*/ Vec3.create();
+const v0_to_v3 = /*@__PURE__*/ Vec3.create();
+const v031_normal = /*@__PURE__*/ Vec3.create();
+const v012_normal = /*@__PURE__*/ Vec3.create();
+const v023_normal = /*@__PURE__*/ Vec3.create();
+const search_v2_dir = /*@__PURE__*/ Vec3.create();
+const search_v3_dir = /*@__PURE__*/ Vec3.create();
+const search_v1_dir = /*@__PURE__*/ Vec3.create();
+
+export function findPortal(
+  result: FindPortalResult,
+  shapeA: SupportShape,
+  shapeB: SupportShape,
+  v0: Vec3,
+  v1: Vec3,
+  v2: Vec3,
+  v3: Vec3,
+  v1_dir_in_support: Vec3,
+  v2_dir_in_support: Vec3,
+  v3_dir_in_support: Vec3,
+  max_iterations: number
+): void {
+  const v0_abs_norm = v0.absNorm();
+  let find_candidate_portal_iteration = 0;
+
+  // The actual loop
+  while (true) {
+    if (find_candidate_portal_iteration >= max_iterations) {
+      result.status = FindPortalStatus.IterationLimit;
+      return;
+    }
+
+    // update iteration data
+    find_candidate_portal_iteration++;
+    v0_to_v1.subtractVectors(v1, v0);
+    v0_to_v2.subtractVectors(v2, v0);
+    v0_to_v3.subtractVectors(v3, v0);
+
+    // update the corresponded vertex
+    // these normal are not oriented
+    v031_normal.crossVectors(v0_to_v3, v0_to_v1);
+    v012_normal.crossVectors(v0_to_v1, v0_to_v2);
+    const signed_volume = v0_to_v2.dot(v031_normal);
+
+    // orient it
+    if (signed_volume < 0) {
+      v2.swap(v3);
+      v2_dir_in_support.swap(v3_dir_in_support);
+      v0_to_v2.swap(v0_to_v3);
+
+      // something tricky here, note the changing of vectors
+      v012_normal.swap(v031_normal);
+      v031_normal.negateVector(v031_normal);
+      v012_normal.negateVector(v012_normal);
+    }
+
+    // case 031
+    // #v-ifdef DEV
+    assert(v0_to_v2.dot(v031_normal) >= 0, "v0_to_v2 and v031_normal should be in the same direction");
+    // #v-endif
+    const v031_seperated_v2_and_o = v0.dot(v031_normal) > dot_eps_ratio * v0_abs_norm * v031_normal.absNorm();
+
+    if (v031_seperated_v2_and_o) {
+      // Orient the normal towards O
+      // #v-ifdef DEV
+      assert(v0.dot(v031_normal) > 0, "o_to_v0 and v031_normal should be in the same direction");
+      // #v-endif
+      search_v2_dir.negateVector(v031_normal);
+
+      // Find a new v2 in that direction
+      support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, search_v2_dir);
+      v2.copy(support.point);
+      v2_dir_in_support.copy(search_v2_dir);
+
+      // Miss detection
+      if (support.isOriginBeyondSupportPoint) {
+        result.status = FindPortalStatus.DetectSeperated;
+        return;
+      }
+
+      continue;
+    }
+
+    // case 012
+    // #v-ifdef DEV
+    assert(v0_to_v3.dot(v012_normal) >= 0, "v0_to_v3 and v012_normal should be in the same direction");
+    // #v-endif
+    const v012_seperated_v3_and_o = v0.dot(v012_normal) > dot_eps_ratio * v0_abs_norm * v012_normal.absNorm();
+
+    if (v012_seperated_v3_and_o) {
+      // Orient the normal towards O
+      // #v-ifdef DEV
+      assert(v0.dot(v012_normal) > 0, "o_to_v0 and v012_normal should be in the same direction");
+      // #v-endif
+      search_v3_dir.negateVector(v012_normal);
+
+      // Find a new v3 in that direction
+      support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, search_v3_dir);
+      v3.copy(support.point);
+      v3_dir_in_support.copy(search_v3_dir);
+
+      // Miss detection
+      if (support.isOriginBeyondSupportPoint) {
+        result.status = FindPortalStatus.DetectSeperated;
+        return;
+      }
+
+      continue;
+    }
+
+    // case 023
+    v023_normal.crossVectors(v0_to_v2, v0_to_v3); // this must be done after orienting
+    // #v-ifdef DEV
+    assert(v0_to_v1.dot(v023_normal) >= 0, "v0_to_v1 and v023_normal should be in the same direction");
+    // #v-endif
+    const v023_seperated_v1_and_o = v0.dot(v023_normal) > dot_eps_ratio * v0_abs_norm * v023_normal.absNorm();
+    if (v023_seperated_v1_and_o) {
+      // Orient the normal towards O
+      // #v-ifdef DEV
+      assert(v0.dot(v023_normal) > 0, "o_to_v0 and v023_normal should be in the same direction");
+      // #v-endif
+      search_v1_dir.negateVector(v023_normal);
+
+      // Find a new v1 in that direction
+      support.getSupportPointOnMinkowskiDifference(shapeA, shapeB, search_v1_dir);
+      v1.copy(support.point);
+      v1_dir_in_support.copy(search_v1_dir);
+
+      // Miss detection
+      if (support.isOriginBeyondSupportPoint) {
+        result.status = FindPortalStatus.DetectSeperated;
+        return;
+      }
+
+      continue;
+    }
+
+    // no seperation found, we're done
+    result.status = FindPortalStatus.PortalFound;
+    return;
+  }
+}

package/src/collision/mpr/mpr.ts

@@ -0,0 +1,29 @@
+export const enum FindPortalStatus {
+  IterationLimit,
+  DetectSeperated,
+  PortalFound,
+}
+
+export const enum MprStatus {
+  NewDirection,
+  SubroutineConverge,
+  Degenerated,
+  Failed,
+  FailedNoIntersect,
+}
+
+export class MprResult {
+  status: MprStatus;
+
+  constructor() {
+    this.status = MprStatus.NewDirection;
+  }
+}
+
+export class FindPortalResult {
+  status: FindPortalStatus;
+
+  constructor() {
+    this.status = FindPortalStatus.IterationLimit;
+  }
+}

package/src/collision/mpr/updatePortal.ts

@@ -0,0 +1,52 @@
+import { Vec3 } from "../../math/vec3";
+
+const v0_v4_o_normal = /*@__PURE__*/ Vec3.create();
+
+export function updatePortal(
+  v0: Vec3,
+  v4: Vec3,
+  v123_normal: Vec3,
+  v1: Vec3,
+  v2: Vec3,
+  v3: Vec3,
+  v1_dir_in_support: Vec3,
+  v2_dir_in_support: Vec3,
+  v3_dir_in_support: Vec3
+): void {
+  // v4 must appear in the next portal
+  // select two in v1, v2 and v3
+  // First do a separation in with plane v0_v4_o
+  v0_v4_o_normal.crossVectors(v4, v0);
+  let dot = v1.dot(v0_v4_o_normal);
+  if (dot > 0) {
+    dot = v2.dot(v0_v4_o_normal);
+    if (dot > 0) {
+      // Discard v1
+      v1.copy(v4);
+      if (v1_dir_in_support !== undefined) {
+        v1_dir_in_support.copy(v123_normal);
+      }
+    } else {
+      // Discard v3
+      v3.copy(v4);
+      if (v3_dir_in_support !== undefined) {
+        v3_dir_in_support.copy(v123_normal);
+      }
+    }
+  } else {
+    dot = v3.dot(v0_v4_o_normal);
+    if (dot > 0) {
+      // Discard v2
+      v2.copy(v4);
+      if (v2_dir_in_support !== undefined) {
+        v2_dir_in_support.copy(v123_normal);
+      }
+    } else {
+      // Discard v1
+      v1.copy(v4);
+      if (v1_dir_in_support !== undefined) {
+        v1_dir_in_support.copy(v123_normal);
+      }
+    }
+  }
+}