@perplexdotgg/bounce 1.0.0 → 1.0.2

package/src/physics/broadphase/TriangleMeshBvhTree.ts

@@ -1,379 +0,0 @@
-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

@@ -1,227 +0,0 @@
-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}`;
-}