@perplexdotgg/bounce 1.0.0

package/src/physics/broadphase/TriangleMeshBvhTree.ts

@@ -0,0 +1,379 @@
+import {
+  createClass,
+  LazyReferenceListType,
+  LazyReferenceType,
+  MonomorphInstance,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+  PropertyDefinitionReferenceList,
+  ReferenceListType,
+  WithPool,
+} from "monomorph";
+import { TriangleMesh } from "../../shape/TriangleMesh";
+import { Aabb } from "../../shape/Aabb";
+import { Vec3 } from "../../math/vec3";
+import { Triangle } from "../../shape/Triangle";
+import { Ray } from "../../shape/Ray";
+
+export interface TriangleMeshBvhTreeOptions {
+  //
+}
+
+export interface TriangleMeshBvhTreeBuildParams {
+  mesh: TriangleMesh;
+  bvhTreeOptions: TriangleMeshBvhTreeOptions;
+}
+
+const bvhNodeProps = {
+  parent: LazyReferenceType((() => TriangleMeshBvhNode) as () => never) as PropertyDefinitionReference<TriangleMeshBvhNode | null, true>,
+  left: LazyReferenceType((() => TriangleMeshBvhNode) as () => never) as PropertyDefinitionReference<TriangleMeshBvhNode | null, true>,
+  right: LazyReferenceType((() => TriangleMeshBvhNode) as () => never) as PropertyDefinitionReference<TriangleMeshBvhNode | null, true>,
+  computedBounds: MonomorphType(Aabb, undefined, true),
+  depth: NumberType(0, true),
+  objects: LazyReferenceListType((() => Triangle) as () => never) as PropertyDefinitionReferenceList<Triangle, Triangle extends MonomorphInstance<infer P, infer I> ? P : never, Triangle extends MonomorphInstance<infer P, infer I> ? I : never>,
+} as const satisfies PropertyDefinitionMap;
+
+export class TriangleMeshBvhNode extends createClass<TriangleMeshBvhNode, typeof bvhNodeProps>(bvhNodeProps) {
+  /**
+   * if bvh node is an internal node, this will be an empty list
+   * if bvh node is a leaf node, this will contain refs to the objects in the leaf
+   */
+  // objects: Triangle[] = [];
+
+  isLeaf() {
+    return this.left === null && this.right === null;
+  }
+
+  isRoot() {
+    return this.parent === null;
+  }
+}
+
+function computeBounds(out: Aabb, objects: Triangle[]) {
+  out.copy(objects[0].computedBounds);
+  for (let i = 1; i < objects.length; i++) {
+    out.unionAabb(objects[i].computedBounds);
+  }
+}
+
+const centroidBounds = /*@__PURE__*/ Aabb.create();
+const extent = /*@__PURE__*/ Vec3.create();
+
+function sortByX(a: Triangle, b: Triangle) {
+  return a.computedBounds.centroid.x - b.computedBounds.centroid.x;
+}
+
+function sortByY(a: Triangle, b: Triangle) {
+  return a.computedBounds.centroid.y - b.computedBounds.centroid.y;
+}
+
+function sortByZ(a: Triangle, b: Triangle) {
+  return a.computedBounds.centroid.z - b.computedBounds.centroid.z;
+}
+
+const sortByAxisFunctions = {
+  0: sortByX,
+  1: sortByY,
+  2: sortByZ,
+} as const;
+
+function splitObjects(objects: Triangle[]) {
+  if (objects.length < 2) {
+    throw new Error("Cannot split fewer than 2 objects.");
+  }
+
+  // expand centroid bounds
+  centroidBounds.min.fromArray([+Infinity, +Infinity, +Infinity]);
+  centroidBounds.max.fromArray([-Infinity, -Infinity, -Infinity]);
+
+  for (const obj of objects) {
+    centroidBounds.expandToPoint(obj.computedBounds.centroid);
+  }
+
+  centroidBounds.computeExtents(extent);
+  const axis = centroidBounds.computeLargestAxis();
+
+  objects.sort(sortByAxisFunctions[axis]);
+  let mid = Math.floor(objects.length / 2);
+  if (mid <= 0 || mid >= objects.length) {
+    mid = 1;
+  }
+
+  const leftObjects = objects.slice(0, mid);
+  const rightObjects = objects.slice(mid);
+  return [leftObjects, rightObjects] as [Triangle[], Triangle[]];
+}
+
+const triangleMeshBvhTreeProps = {
+  nodes: ReferenceListType(TriangleMeshBvhNode),
+} as const satisfies PropertyDefinitionMap;
+
+export class TriangleMeshBvhTree extends createClass<TriangleMeshBvhTree, typeof triangleMeshBvhTreeProps>(
+  triangleMeshBvhTreeProps
+) {
+  // TODO: init this via constructor params
+  intersectStack: TriangleMeshBvhNode[] = [];
+  walkStack: TriangleMeshBvhNode[] = [];
+
+  build(params: TriangleMeshBvhTreeBuildParams, trianglePool: typeof Triangle.Pool) {
+    const objects: Triangle[] = [];
+    for (const triangle of params.mesh.triangles!) {
+      objects.push(triangle);
+    }
+
+    const root = this.nodes.create({
+      objects: {
+        pool: trianglePool,
+      },
+    });
+    this._buildRecursive(objects, root, null);
+  }
+
+  get root() {
+    return this.nodes?.getAtIndex(0);
+  }
+
+  _buildRecursive(objects: Triangle[], node: TriangleMeshBvhNode, parent: TriangleMeshBvhNode | null) {
+    node.parent = parent;
+    node.depth = parent ? parent.depth + 1 : 0;
+    computeBounds(node.computedBounds, objects);
+
+    if (objects.length <= 2) {
+      node.objects.length = 0;
+      for (const object of objects) {
+        node.objects.push(object as WithPool<Triangle>);
+      }
+      return;
+    }
+
+    const [leftObjects, rightObjects] = splitObjects(objects);
+
+    if (leftObjects.length === 0 || rightObjects.length === 0) {
+      node.objects.length = 0;
+      for (const object of objects) {
+        node.objects.push(object as WithPool<Triangle>);
+      }
+      return;
+    }
+
+    const left = this.nodes.create({
+      objects: {
+        pool: node.objects.pool,
+      },
+    });
+    const right = this.nodes.create({
+      objects: {
+        pool: node.objects.pool,
+      },
+    });
+
+    node.left = left;
+    node.right = right;
+
+    this._buildRecursive(leftObjects, left, node);
+    this._buildRecursive(rightObjects, right, node);
+  }
+
+  intersectAabb(onHit: (triangle: Triangle) => boolean, aabb: Aabb) {
+    // console.log("intersect body", body);
+    if (!this.root) {
+      return;
+    }
+
+    const stack = this.intersectStack;
+    stack.length = 0;
+    stack.push(this.root);
+
+    while (stack.length > 0) {
+      const node = stack.pop();
+
+      if (!node) {
+        continue;
+      }
+
+      // skip this node if it doesn't intersect the AABB
+      if (!node.computedBounds.intersectsAabb(aabb)) {
+        continue;
+      }
+
+      // if the node is not a leaf, push its children onto the stack and continue
+      if (!node.isLeaf()) {
+        stack.push(node.left!);
+        stack.push(node.right!);
+        continue;
+      }
+
+      // if the node is a leaf, check its objects
+      for (const object of node.objects) {
+        if (!aabb.intersectsAabb(object.computedBounds)) {
+          continue;
+        }
+
+        if (onHit(object)) {
+          return;
+        }
+      }
+    }
+  }
+
+  walk(onNode: (node: TriangleMeshBvhNode) => boolean) {
+    if (!this.root) {
+      return;
+    }
+
+    const stack = this.walkStack;
+    stack.length = 0;
+    stack.push(this.root);
+
+    while (stack.length > 0) {
+      const node = stack.pop();
+
+      if (!node) {
+        continue;
+      }
+
+      if (onNode(node)) {
+        return;
+      }
+
+      if (!node.isLeaf()) {
+        stack.push(node.left!);
+        stack.push(node.right!);
+      }
+    }
+  }
+
+  computeMaxDepth() {
+    if (!this.root) {
+      return 0;
+    }
+
+    let maxDepth = 0;
+    this.walk(node => {
+      if (node.depth > maxDepth) {
+        maxDepth = node.depth;
+      }
+      return false;
+    });
+    return maxDepth;
+  }
+
+  castAabb(onHit: (triangle: Triangle) => boolean, bounds: Aabb, displacement: Vec3) {
+    // exit if the tree is empty
+    if (!this.root) {
+      return;
+    }
+
+    // aabb cast is done by the following method
+    // 1. shrink the shape aabb by its own extents down to a point
+    // 2. expand the block aabb by the extents of the shape aabb
+    // 3. cast the point by the displacement against the expanded block aabb (raycast vs aabb test)
+
+    bounds.computeCentroid(ray.origin);
+    ray.direction.normalizeVector(displacement);
+    ray.length = displacement.length();
+
+    bounds.computeHalfExtents(halfExtents);
+
+    const stack = this.intersectStack;
+    stack.length = 0;
+    stack.push(this.root);
+
+    while (stack.length > 0) {
+      const node = stack.pop();
+
+      if (!node) {
+        continue;
+      }
+
+      // skip branch if the node's expanded aabb does not intersect with the shape's ray
+      nodeBounds.copy(node.computedBounds);
+      nodeBounds.expandByVector(halfExtents);
+      if (ray.intersectsAabb(nodeBounds) === false) {
+        continue;
+      }
+
+      // if the node is not a leaf, push its children onto the stack and continue
+      if (!node.isLeaf()) {
+        stack.push(node.left!);
+        stack.push(node.right!);
+        continue;
+      }
+
+      // if the node is a leaf, check its objects
+      for (const body of node.objects) {
+        // expand the object aabb by the shape's aabb
+        // skip if bounds do not collide
+        bodyBounds.copy(body.computedBounds);
+        bodyBounds.expandByVector(halfExtents);
+        if (ray.intersectsAabb(bodyBounds) === false) {
+          continue;
+        }
+
+        // visit the body
+        if (onHit(body)) {
+          return;
+        }
+      }
+    }
+  }
+
+  castRay(onHit: (triangle: Triangle) => boolean, ray: Ray) {
+    // exit if the tree is empty
+    if (!this.root) {
+      return;
+    }
+
+    // aabb cast is done by the following method
+    // 1. shrink the shape aabb by its own extents down to a point
+    // 2. expand the block aabb by the extents of the shape aabb
+    // 3. cast the point by the displacement against the expanded block aabb (raycast vs aabb test)
+
+    const stack = this.intersectStack;
+    stack.length = 0;
+    stack.push(this.root);
+
+    while (stack.length > 0) {
+      const node = stack.pop();
+
+      if (!node) {
+        continue;
+      }
+
+      // skip branch if the node's expanded aabb does not intersect with the shape's ray
+      nodeBounds.copy(node.computedBounds);
+      if (ray.intersectsAabb(nodeBounds) === false) {
+        continue;
+      }
+
+      // if the node is not a leaf, push its children onto the stack and continue
+      if (!node.isLeaf()) {
+        stack.push(node.left!);
+        stack.push(node.right!);
+        continue;
+      }
+
+      // if the node is a leaf, check its objects
+      for (const body of node.objects) {
+        // expand the object aabb by the shape's aabb
+        // skip if bounds do not collide
+        bodyBounds.copy(body.computedBounds);
+        if (ray.intersectsAabb(bodyBounds) === false) {
+          continue;
+        }
+
+        // visit the body
+        if (onHit(body)) {
+          return;
+        }
+      }
+    }
+  }
+}
+
+const ray = /*@__PURE__*/ Ray.create();
+const halfExtents = /*@__PURE__*/ Vec3.create();
+const nodeBounds = /*@__PURE__*/ Aabb.create();
+const bodyBounds = /*@__PURE__*/ Aabb.create();

package/src/physics/manifold/ContactManifold.ts

@@ -0,0 +1,227 @@
+import {
+  createClass,
+  InputType,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PoolClass,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+  ReferenceListType,
+  ReferenceType,
+} from "monomorph";
+import { Body } from "../Body";
+import { Vec3 } from "../../math/vec3";
+import { Quat } from "../../math/quat";
+import { Mat4 } from "../../math/mat4";
+import { ReferenceFrame } from "../../constraints/BaseConstraint";
+
+const localToWorldTransformA = /*@__PURE__*/ Mat4.create();
+const point = /*@__PURE__*/ Vec3.create();
+
+const contactManifoldProps = {
+  // meta data
+  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),
+  nextContactManifold: LazyReferenceType((() => ContactManifold) as () => never) as PropertyDefinitionReference<
+    ContactManifold | null,
+    true
+  >,
+  // geometric data
+  baseTranslation: MonomorphType(Vec3),
+  firstWorldSpaceNormal: MonomorphType(Vec3),
+  worldSpaceNormal: MonomorphType(Vec3),
+  penetrationDepth: NumberType(0.0),
+  numContacts: NumberType(0),
+  contactPointsA: ReferenceListType(Vec3),
+  contactPointsB: ReferenceListType(Vec3),
+  lambdas: ReferenceListType(Vec3),
+} as const satisfies PropertyDefinitionMap;
+
+export class ContactManifold extends createClass<ContactManifold, typeof contactManifoldProps>(contactManifoldProps) {
+  get maxContacts(): number {
+    return this.contactPointsA!.maxLength;
+  }
+
+  pushContactPoints(pointA: Vec3, pointB: Vec3): void {
+    if (this.numContacts >= this.maxContacts) {
+      throw new Error("contact manifold is full");
+    }
+
+    this.contactPointsA!.getAtIndex(this.numContacts)!.copy(pointA);
+    this.contactPointsB!.getAtIndex(this.numContacts)!.copy(pointB);
+    // this.lambdas!.getAtIndex(this.numContacts)!.copy(pointA);
+    this.numContacts++;
+  }
+
+  get key(): string {
+    return createContactPairKey(this.bodyA!, this.bodyB!);
+  }
+
+  copyLambda(other: ContactManifold, index: number): void {
+    this.lambdas.getAtIndex(index)!.copy(other.lambdas.getAtIndex(index)!);
+  }
+
+  copyLambdas(other: ContactManifold): void {
+    for (let i = 0; i < this.numContacts; i++) {
+      this.lambdas.getAtIndex(i)!.copy(other.lambdas.getAtIndex(i)!);
+    }
+  }
+
+  resetLambdas(): void {
+    for (let i = 0; i < this.numContacts; i++) {
+      this.lambdas.getAtIndex(i)!.zero();
+    }
+  }
+
+  getContactPointA(out: Vec3, index: number): void {
+    out.copy(this.contactPointsA.getAtIndex(index)!);
+  }
+
+  getContactPointB(out: Vec3, index: number): void {
+    out.copy(this.contactPointsB.getAtIndex(index)!);
+  }
+
+  setContactPointA(point: Vec3, index: number): void {
+    this.contactPointsA.getAtIndex(index)!.copy(point);
+  }
+
+  setContactPointB(point: Vec3, index: number): void {
+    this.contactPointsB.getAtIndex(index)!.copy(point);
+  }
+
+  clear() {
+    this.numContacts = 0;
+  }
+
+  /**
+   * this function assumes that the contact points are stored in the local space of the corresponding body!
+   */
+  getWorldContactPointA(out: Vec3, index: number): void {
+    const bodyA = this.bodyA!;
+    localToWorldTransformA.fromRotationTranslation(bodyA.orientation, bodyA.computedCenterOfMassPosition);
+
+    this.getContactPointA(out, index);
+    out.transformVectorFromMat4(out, localToWorldTransformA);
+  }
+
+  getContactPointAInSpace(out: Vec3, index: number, space: ReferenceFrame): void {
+    if (space === ReferenceFrame.world) {
+      this.getWorldContactPointA(out, index);
+    } else {
+      this.getContactPointA(out, index);
+    }
+  }
+
+  getCentroidA(out: Vec3): void {
+    out.zero();
+
+    for (let i = 0; i < this.numContacts; i++) {
+      this.getContactPointA(point, i);
+      out.addVectors(out, point);
+    }
+
+    out.scaleVector(out, 1 / this.numContacts);
+  }
+
+  getWorldCentroidA(out: Vec3): void {
+    const bodyA = this.bodyA!;
+    localToWorldTransformA.fromRotationTranslation(bodyA.orientation, bodyA.computedCenterOfMassPosition);
+
+    this.getCentroidA(out);
+    out.transformVectorFromMat4(out, localToWorldTransformA);
+  }
+
+  getCentroidAInSpace(out: Vec3, space: ReferenceFrame): void {
+    if (space === ReferenceFrame.world) {
+      this.getWorldCentroidA(out);
+    } else {
+      this.getCentroidA(out);
+    }
+  }
+
+  containsBody(body: Body): boolean {
+    return this.bodyA === body || this.bodyB === body;
+  }
+}
+
+export class ContactManifoldPool {
+  // the contact manifold pool
+  contactManifoldPool: PoolClass<ContactManifold>;
+  vec3Pool: PoolClass<Vec3>;
+
+  numMaxManifolds: number;
+  maxNumContacts: number;
+
+  constructor(numMaxManifolds: number, numMaxContacts: number) {
+    this.contactManifoldPool = new ContactManifold.Pool();
+    this.vec3Pool = new Vec3.Pool();
+
+    this.numMaxManifolds = numMaxManifolds;
+    this.maxNumContacts = numMaxContacts;
+  }
+
+  createContactManifold(data: InputType<ContactManifold>): ContactManifold {
+    data = data ?? {};
+
+    if (data.contactPointsA) {
+      data.contactPointsA.pool = this.vec3Pool;
+    } else {
+      data.contactPointsA = { pool: this.vec3Pool };
+    }
+
+    if (data.contactPointsB) {
+      data.contactPointsB.pool = this.vec3Pool;
+    } else {
+      data.contactPointsB = { pool: this.vec3Pool };
+    }
+
+    if (data.lambdas) {
+      data.lambdas.pool = this.vec3Pool;
+    } else {
+      data.lambdas = { pool: this.vec3Pool };
+    }
+
+    const manifold = ContactManifold.create(data, this.contactManifoldPool);
+
+    // todo this behavior probably should change?
+    // currently, we create all the points once the first time, and never destroy them.
+    // they technically should be destroyed, but this is more efficient for this use case
+    // since we will be reusing the same manifolds over and over again, so we should just
+    // increment a numManifolds instead that is reset to 0
+    if (manifold.contactPointsA.length >= this.maxNumContacts) {
+      return manifold;
+    }
+
+
+    for (let i = 0; i < this.maxNumContacts; i++) {
+      manifold.contactPointsA.create();
+      manifold.contactPointsB.create();
+      manifold.lambdas.create();
+    }
+
+    return manifold;
+  }
+}
+
+const contactPairProps = {
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  firstContactManifold: ReferenceType(ContactManifold),
+  translationAB: MonomorphType(Vec3),
+  rotationAB: MonomorphType(Quat),
+} as const satisfies PropertyDefinitionMap;
+
+export class ContactPair extends createClass<ContactPair, typeof contactPairProps>(contactPairProps) {
+  get key(): string {
+    return createContactPairKey(this.bodyA!, this.bodyB!);
+  }
+}
+
+export function createContactPairKey(bodyA: Body, bodyB: Body): string {
+  const keyA = `${bodyA.type}:${bodyA.index}`;
+  const keyB = `${bodyB.type}:${bodyB.index}`;
+  return keyA < keyB ? `${keyA}|${keyB}` : `${keyB}|${keyA}`;
+}