@perplexdotgg/bounce 1.0.0

package/src/physics/SleepModule.ts

@@ -0,0 +1,163 @@
+import { Body, BodyType } from "./Body";
+import { BodyPairNode, BodyPairsModule } from "./broadphase/BodyPairsModule";
+import { ConstraintPairNode, ConstraintPairsModule } from "./broadphase/ConstraintPairsModule";
+
+function wakeBodyPair(node: BodyPairNode) {
+  node.bodyA!.isSleeping = false;
+  node.bodyA!.timeWithoutMoving = 0;
+
+  node.bodyB!.isSleeping = false;
+  node.bodyB!.timeWithoutMoving = 0;
+  return false;
+}
+
+export interface SleepOptions {
+  isSleepingEnabled: boolean;
+  sleepVelocityThreshold: number;
+  sleepSpinThreshold: number;
+  sleepTimeThreshold: number;
+}
+
+export class SleepModule {
+  options: SleepOptions;
+  contactPairs: BodyPairsModule;
+  constraintPairs: ConstraintPairsModule;
+
+  _areAllConnectedBodyPairsReadyToSleep: boolean;
+
+  constructor(contactPairs: BodyPairsModule, constraintPairs: ConstraintPairsModule, options?: Partial<SleepOptions>) {
+    this.options = {
+      isSleepingEnabled: true,
+      sleepVelocityThreshold: 0.03,
+      sleepSpinThreshold: 0.03,
+      sleepTimeThreshold: 0.5,
+      ...(options || {}),
+    };
+    this.contactPairs = contactPairs;
+    this.constraintPairs = constraintPairs;
+
+    this._areAllConnectedBodyPairsReadyToSleep = true;
+  }
+
+  wakeBodyUp(body: Body) {
+    if (!body.isSleeping) {
+      return;
+    }
+
+    body.isSleeping = false;
+    body.timeWithoutMoving = 0;
+
+    if (body.firstPotentialPairEdge) {
+      this.contactPairs.walk(body.firstPotentialPairEdge.node!, wakeBodyPair);
+    }
+
+    if (body.firstPotentialConstraintPairEdge) {
+      this.constraintPairs.walk(body.firstPotentialConstraintPairEdge.node!, wakeBodyPair);
+    }
+  }
+
+  updateBodySleepStates(bodies: typeof Body.Pool | typeof Body.ReferenceList, timeStepSizeSeconds: number) {
+    if (!this.options.isSleepingEnabled) {
+      return;
+    }
+
+    const sleepVelocityThresholdSquared = this.options.sleepVelocityThreshold ** 2;
+    const sleepSpinThresholdSquared = this.options.sleepSpinThreshold ** 2;
+
+    for (const body of bodies) {
+      const velocityMagnitudeSquared = body.linearVelocity.squaredLength();
+      const spinMagnitudeSquared = body.angularVelocity.squaredLength();
+
+      if (!body.isSleepingEnabled) {
+        continue;
+      }
+
+      if (
+        velocityMagnitudeSquared < sleepVelocityThresholdSquared &&
+        spinMagnitudeSquared < sleepSpinThresholdSquared
+      ) {
+        body.timeWithoutMoving += timeStepSizeSeconds;
+      } else {
+        body.timeWithoutMoving = 0;
+      }
+    }
+
+    buildAndSleepIslands(bodies, this.options.sleepTimeThreshold);
+  }
+
+  isBodyPairAwake(bodyA: Body, bodyB: Body) {
+    return bodyA.isSleeping === false && bodyB.isSleeping === false;
+  }
+}
+
+let sleepVisitGeneration = 0;
+const stack: Body[] = [];
+const island: Body[] = [];
+
+function buildAndSleepIslands(bodies: typeof Body.Pool | typeof Body.ReferenceList, sleepTimeThreshold: number) {
+  const visitGeneration = ++sleepVisitGeneration;
+
+  for (const seedBody of bodies) {
+    if (seedBody.type === BodyType.static) {
+      continue;
+    }
+    if (seedBody.visitGeneration === visitGeneration) {
+      continue;
+    }
+    if (seedBody.isSleeping) {
+      continue;
+    }
+
+    // start a new island from this body
+    stack.length = 0;
+    stack.push(seedBody);
+    island.length = 0;
+    let allReady = true;
+
+    while (stack.length) {
+      const body = stack.pop()!;
+      if (body.visitGeneration === visitGeneration) {
+        continue;
+      }
+
+      body.visitGeneration = visitGeneration;
+      island.push(body);
+
+      const isCurrentBodyReadyToSleep = body.isReadyToSleep(sleepTimeThreshold);
+      if (!isCurrentBodyReadyToSleep) {
+        allReady = false;
+      }
+
+      // traverse contacts
+      for (let e = body.firstPotentialPairEdge; e; e = e.next) {
+        const node = e.node!;
+        const other = node.bodyA === body ? node.bodyB! : node.bodyA!;
+        if (other.type !== BodyType.static && other.visitGeneration !== visitGeneration) {
+          stack.push(other);
+        }
+      }
+
+      // traverse constraints
+      for (let e = body.firstPotentialConstraintPairEdge; e; e = e.next) {
+        const node = e.node!;
+        const other = node.bodyA === body ? node.bodyB! : node.bodyA!;
+        if (other.type !== BodyType.static && other.visitGeneration !== visitGeneration) {
+          stack.push(other);
+        }
+      }
+    }
+
+    // set sleep for the entire island
+    if (allReady) {
+      for (const body of island) {
+        // body.sleep();
+        body.isSleeping = true;
+      }
+    } else {
+      for (const body of island) {
+        // body.wakeUp();
+        body.isSleeping = false;
+      }
+    }
+  }
+}

package/src/physics/broadphase/BodyPairsModule.ts

@@ -0,0 +1,363 @@
+import {
+  createClass,
+  LazyReferenceType,
+  PoolClass,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { Body, BodyType } from "../Body";
+
+const bodyPairNodeProps = {
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  edgeA: LazyReferenceType((() => BodyPairEdge) as () => never) as PropertyDefinitionReference<
+    BodyPairEdge | null,
+    true
+  >,
+  edgeB: LazyReferenceType((() => BodyPairEdge) as () => never) as PropertyDefinitionReference<
+    BodyPairEdge | null,
+    true
+  >,
+} as const satisfies PropertyDefinitionMap;
+
+export class BodyPairNode extends createClass<BodyPairNode, typeof bodyPairNodeProps>(bodyPairNodeProps) {
+  containsStaticBody() {
+    return this.bodyA!.type === BodyType.static || this.bodyB!.type === BodyType.static;
+  }
+}
+
+const bodyPairEdgeProps = {
+  node: LazyReferenceType((() => BodyPairNode) as () => never) as PropertyDefinitionReference<
+    BodyPairNode | null,
+    true
+  >,
+  next: LazyReferenceType((() => BodyPairEdge) as () => never) as PropertyDefinitionReference<
+    BodyPairEdge | null,
+    true
+  >,
+  prev: LazyReferenceType((() => BodyPairEdge) as () => never) as PropertyDefinitionReference<
+    BodyPairEdge | null,
+    true
+  >,
+} as const satisfies PropertyDefinitionMap;
+
+export class BodyPairEdge extends createClass<BodyPairEdge, typeof bodyPairEdgeProps>(bodyPairEdgeProps) {}
+
+export class BodyPairsModule {
+  bodyPool: PoolClass<Body>;
+  nodesPool: PoolClass<BodyPairNode>;
+  edgesPool: PoolClass<BodyPairEdge>;
+
+  walkStack: BodyPairNode[];
+  walkVisited: Set<BodyPairNode>;
+
+  bodiesToSleep: Set<Body>;
+  _sleepTimeThreshold: number;
+  _allReady: boolean;
+
+  constructor(bodyPool: PoolClass<Body>, nodesPool: PoolClass<BodyPairNode>, edgesPool: PoolClass<BodyPairEdge>) {
+    this.bodyPool = bodyPool;
+    this.nodesPool = nodesPool;
+    this.edgesPool = edgesPool;
+
+    this.walkStack = [];
+    this.walkVisited = new Set();
+
+    this.bodiesToSleep = new Set();
+    this._sleepTimeThreshold = 0.5;
+    this._allReady = true;
+  }
+
+  toArray(array: number[] | Float32Array | Float64Array, startOffset = 0): number {
+    startOffset = this.nodesPool.toArray(array, startOffset);
+    startOffset = this.edgesPool.toArray(array, startOffset);
+    return startOffset;
+  }
+
+  fromArray(array: number[] | Float32Array | Float64Array, startOffset = 0): number {
+    let initialStartOffset = startOffset;
+
+    // everything but references, since we have circular references
+    // we do the references below
+    startOffset = this.edgesPool.fromArrayNoReferences(array, startOffset);
+
+    startOffset = this.nodesPool.fromArray(array, {
+      bodyA: this.bodyPool,
+      bodyB: this.bodyPool,
+      edgeA: this.edgesPool,
+      edgeB: this.edgesPool,
+    }, startOffset);
+
+    this.edgesPool.fromArrayOnlyReferences(array, {
+      node: this.nodesPool,
+      next: this.edgesPool,
+      prev: this.edgesPool,
+    }, initialStartOffset);
+
+    return startOffset;
+  }
+
+  _checkReadyToSleep = (neighbor: BodyPairNode) => {
+    this.walkVisited.add(neighbor);
+    const bodyA = neighbor.bodyA!;
+    const bodyB = neighbor.bodyB!;
+
+    if (bodyA.type === BodyType.dynamic) {
+      this.bodiesToSleep.add(bodyA);
+      if (!bodyA.isReadyToSleep(this._sleepTimeThreshold)) {
+        this._allReady = false;
+        return true;
+      }
+    }
+
+    if (bodyB.type === BodyType.dynamic) {
+      this.bodiesToSleep.add(bodyB);
+      if (!bodyB.isReadyToSleep(this._sleepTimeThreshold)) {
+        this._allReady = false;
+        return true;
+      }
+    }
+
+    if (
+      !neighbor.bodyA!.isReadyToSleep(this._sleepTimeThreshold) ||
+      !neighbor.bodyB!.isReadyToSleep(this._sleepTimeThreshold)
+    ) {
+      this._allReady = false;
+      return true;
+    }
+    return false;
+  };
+
+  /**
+   * creates a body pair. O(1)
+   * @param bodyA
+   * @param bodyB
+   * @returns
+   */
+  createPair(bodyA: Body, bodyB: Body) {
+    // if like-type, ensure bodyA has the lower index
+    if (bodyA.type === bodyB.type && bodyA.index > bodyB.index) {
+      const temp = bodyA;
+      bodyA = bodyB;
+      bodyB = temp;
+    }
+    // if unlike-type, ensure bodyA is the dynamic
+    if (bodyA.type !== bodyB.type && bodyA.type !== BodyType.dynamic) {
+      const temp = bodyA;
+      bodyA = bodyB;
+      bodyB = temp;
+    }
+
+    // allocations
+    const node = /*@__PURE__*/ BodyPairNode.create(undefined, this.nodesPool);
+    node.bodyA = null;
+    node.bodyB = null;
+    node.edgeA = null;
+    node.edgeB = null;
+
+    const edgeA = /*@__PURE__*/ BodyPairEdge.create(undefined, this.edgesPool);
+    edgeA.next = null;
+    edgeA.prev = null;
+    edgeA.node = null;
+
+    const edgeB = /*@__PURE__*/ BodyPairEdge.create(undefined, this.edgesPool);
+    edgeB.next = null;
+    edgeB.prev = null;
+    edgeB.node = null;
+
+    // wire up node
+    node.bodyA = bodyA;
+    node.bodyB = bodyB;
+    node.edgeA = edgeA;
+    node.edgeB = edgeB;
+
+    // wire up edgeA
+    edgeA.node = node;
+    edgeA.prev = null;
+    edgeA.next = bodyA.firstPotentialPairEdge;
+    if (bodyA.firstPotentialPairEdge) {
+      bodyA.firstPotentialPairEdge.prev = edgeA;
+    }
+    bodyA.firstPotentialPairEdge = edgeA;
+
+    // wire up edgeB
+    edgeB.node = node;
+    edgeB.prev = null;
+    edgeB.next = bodyB.firstPotentialPairEdge;
+    if (bodyB.firstPotentialPairEdge) {
+      bodyB.firstPotentialPairEdge.prev = edgeB;
+    }
+    bodyB.firstPotentialPairEdge = edgeB;
+
+    return node;
+  }
+
+  /**
+   * destroys a body pair. O(k) where k is the number of pairs involving bodyA
+   */
+  destroyPair(bodyA: Body, bodyB: Body) {
+    let edge = bodyA.firstPotentialPairEdge;
+
+    // walk through the edges of bodyA
+    while (edge) {
+      // find the node
+      const node = edge.node!;
+      if (node.bodyB !== bodyB) {
+        edge = edge.next;
+        continue;
+      }
+
+      // unlink edgeA from bodyA
+      const edgeA = node.edgeA!;
+      if (edgeA.prev) {
+        edgeA.prev.next = edgeA.next;
+      } else {
+        bodyA.firstPotentialPairEdge = edgeA.next;
+      }
+      if (edgeA.next) {
+        edgeA.next.prev = edgeA.prev;
+      }
+
+      // unlink edgeB from bodyB
+      const edgeB = node.edgeB!;
+      if (edgeB.prev) {
+        edgeB.prev.next = edgeB.next;
+      } else {
+        bodyB.firstPotentialPairEdge = edgeB.next;
+      }
+      if (edgeB.next) {
+        edgeB.next.prev = edgeB.prev;
+      }
+
+      // destroy the objects
+      edgeA.destroy();
+      edgeB.destroy();
+      node.destroy();
+
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * destroys all pairs of involving a single body. O(k) where k is the number of pairs involving body
+   */
+  destroyAllPairsOfOne(body: Body) {
+    let destroyed = false;
+
+    // walk through the edges of body
+    let edge = body.firstPotentialPairEdge;
+    while (edge) {
+      const nextEdge = edge.next as BodyPairEdge | null;
+
+      const node = edge.node!;
+      const edgeA = node.edgeA!;
+      const edgeB = node.edgeB!;
+
+      const otherBody = node.bodyA === body ? node.bodyB! : node.bodyA!;
+      const otherEdge = node.bodyA === body ? edgeB : edgeA;
+
+      // unlink the other edge from the other body
+      if (otherEdge.prev) {
+        otherEdge.prev.next = otherEdge.next;
+      } else {
+        otherBody.firstPotentialPairEdge = otherEdge.next;
+      }
+      if (otherEdge.next) {
+        otherEdge.next.prev = otherEdge.prev;
+      }
+
+      // destroy the objects
+      edgeA.destroy();
+      edgeB.destroy();
+      node.destroy();
+
+      edge = nextEdge;
+      destroyed = true;
+    }
+
+    body.firstPotentialPairEdge = null;
+
+    return destroyed;
+  }
+
+  *iteratePairsOfOne(body: Body): IterableIterator<BodyPairNode> {
+    let edge = body.firstPotentialPairEdge;
+    while (edge) {
+      yield edge.node!;
+      edge = edge.next;
+    }
+  }
+
+  *iteratePairs(): IterableIterator<BodyPairNode> {
+    for (const node of this.nodesPool) {
+      yield node;
+    }
+  }
+
+  walk(startNode: BodyPairNode, onNode: (node: BodyPairNode) => boolean) {
+    const visited = this.walkVisited;
+    visited.clear();
+
+    const stack = this.walkStack;
+    stack.length = 0;
+    stack.push(startNode);
+
+    while (stack.length > 0) {
+      const node = stack.pop()!;
+      if (visited.has(node)) {
+        continue;
+      }
+      visited.add(node);
+
+      if (onNode(node)) {
+        return;
+      }
+
+      const bodyA = node.bodyA!;
+      const bodyB = node.bodyB!;
+
+      // only walk through dynamic bodies — prevents static bridges
+      if (bodyA.type === BodyType.dynamic) {
+        for (const neighbor of this.iteratePairsOfOne(bodyA)) {
+          if (!visited.has(neighbor)) {
+            stack.push(neighbor);
+          }
+        }
+      }
+
+      if (bodyB.type === BodyType.dynamic) {
+        for (const neighbor of this.iteratePairsOfOne(bodyB)) {
+          if (!visited.has(neighbor)) {
+            stack.push(neighbor);
+          }
+        }
+      }
+    }
+  }
+
+  sleepAllReadyBodies(sleepTimeThreshold: number) {
+    this.walkVisited.clear();
+    this.bodiesToSleep.clear();
+    this._sleepTimeThreshold = sleepTimeThreshold;
+    this._allReady = true;
+
+    for (const node of this.nodesPool) {
+      if (this.walkVisited.has(node)) {
+        continue;
+      }
+
+      this.bodiesToSleep.clear();
+      this._allReady = true;
+
+      this.walk(node, this._checkReadyToSleep);
+
+      if (this._allReady) {
+        for (const body of this.bodiesToSleep) {
+          body.sleep();
+        }
+      }
+    }
+  }
+}