@perplexdotgg/bounce 1.0.0 → 1.0.1

package/src/physics/solver/ManifoldConstraint.ts

@@ -1,420 +0,0 @@
-import {
-  createClass,
-  InputType,
-  LazyReferenceType,
-  MonomorphType,
-  NumberType,
-  PoolClass,
-  PropertyDefinitionMap,
-  PropertyDefinitionReference,
-  ReferenceListType,
-  ReferenceType,
-} from "monomorph";
-import { Body, BodyType } from "../Body";
-import { Vec3 } from "../../math/vec3";
-import { ContactManifold, createContactPairKey } from "../manifold/ContactManifold";
-import { Mat3 } from "../../math/mat3";
-
-const linearVelocityA = /*@__PURE__*/ Vec3.create();
-const linearVelocityB = /*@__PURE__*/ Vec3.create();
-const linearVelocityBA = /*@__PURE__*/ Vec3.create();
-const angularVelocityA = /*@__PURE__*/ Vec3.create();
-const angularVelocityB = /*@__PURE__*/ Vec3.create();
-
-const inverseInertiaA = /*@__PURE__*/ Mat3.create();
-const inverseInertiaB = /*@__PURE__*/ Mat3.create();
-
-const directionalConstraintProps = {
-  mR1PlusUxAxis: MonomorphType(Vec3),
-  mR2xAxis: MonomorphType(Vec3),
-  mInvI1_R1PlusUxAxis: MonomorphType(Vec3),
-  mInvI2_R2xAxis: MonomorphType(Vec3),
-  effectiveMass: NumberType(0.0),
-  bias: NumberType(0.0),
-  softness: NumberType(0.0),
-  totalLambda: NumberType(0.0),
-} as const satisfies PropertyDefinitionMap;
-
-export class DirectionalConstraint extends createClass<DirectionalConstraint, typeof directionalConstraintProps>(
-  directionalConstraintProps
-) {
-  isSpringActive(): boolean {
-    return this.softness !== 0;
-  }
-
-  computeInverseEffectiveMassWithMassOverride(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    momentArmA: Vec3,
-    momentArmB: Vec3,
-    direction: Vec3
-  ): number {
-    // recompute the inverse inertia tensors
-    bodyA.computeInverseInertiaTensor(inverseInertiaA);
-    bodyB.computeInverseInertiaTensor(inverseInertiaB);
-
-    return this.computeInverseEffectiveMass(
-      bodyA,
-      bodyB,
-      inverseMassA,
-      inverseMassB,
-      inverseInertiaA,
-      inverseInertiaB,
-      momentArmA,
-      momentArmB,
-      direction
-    );
-  }
-
-  initializeWithMassOverride(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    momentArmA: Vec3,
-    momentArmB: Vec3,
-    direction: Vec3,
-    bias: number = 0
-  ): void {
-    const inverseEffectiveMass = this.computeInverseEffectiveMassWithMassOverride(
-      bodyA,
-      bodyB,
-      inverseMassA,
-      inverseMassB,
-      momentArmA,
-      momentArmB,
-      direction
-    );
-
-    if (inverseEffectiveMass === 0) {
-      this.effectiveMass = 0;
-      this.totalLambda = 0;
-      return;
-    }
-
-    this.effectiveMass = 1 / inverseEffectiveMass;
-    this.bias = bias;
-    this.softness = 0;
-  }
-
-  applyImpulse(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    direction: Vec3,
-    inTotalLambda: number
-  ): void {
-    const deltaLambda = inTotalLambda - this.totalLambda;
-    this.totalLambda = inTotalLambda;
-
-    // if lambda is zero, do nothing
-    if (deltaLambda === 0) {
-      return;
-    }
-
-    if (bodyA.type === BodyType.dynamic) {
-      bodyA.linearVelocity.addScaled(direction, -deltaLambda * inverseMassA);
-      // console.log("applying delta-v of magnitude", -deltaLambda * inverseMassA, "in direction", direction.toObject());
-      bodyA.angularVelocity.addScaled(this.mInvI1_R1PlusUxAxis, -deltaLambda);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      bodyB.linearVelocity.addScaled(direction, +deltaLambda * inverseMassB);
-      bodyB.angularVelocity.addScaled(this.mInvI2_R2xAxis, +deltaLambda);
-    }
-  }
-  getBias(totalLambda: number): number {
-    return this.softness * totalLambda + this.bias;
-  }
-
-  getTotalLambda(bodyA: Body, bodyB: Body, direction: Vec3): number {
-    // #v-ifdef DEV
-    // assert at least one body is dynamic
-    if (bodyA.type !== BodyType.dynamic && bodyB.type !== BodyType.dynamic) {
-      throw new Error("at least one body must be dynamic");
-    }
-    // #v-endif
-    // calculate the jacobian multiplied by the velocity
-    let jv = 0;
-
-    // jacobian multiplied by the linear relative velocity
-    linearVelocityA.copy(bodyA.linearVelocity);
-    linearVelocityB.copy(bodyB.linearVelocity);
-    linearVelocityBA.subtractVectors(linearVelocityA, linearVelocityB);
-
-    jv += direction.dot(linearVelocityBA);
-
-    // jacobian multiplied by the angular relative velocity
-    angularVelocityA.copy(bodyA.angularVelocity);
-    angularVelocityB.copy(bodyB.angularVelocity);
-
-    if (bodyA.type !== BodyType.static) {
-      jv += this.mR1PlusUxAxis.dot(angularVelocityA);
-    }
-
-    if (bodyB.type !== BodyType.static) {
-      jv -= this.mR2xAxis.dot(angularVelocityB);
-    }
-
-    // compute lambda = -K^-1 * (jv - b)
-    const lambda = this.effectiveMass * (jv - this.getBias(this.totalLambda));
-    const accumulatedLambda = this.totalLambda + lambda;
-    return accumulatedLambda;
-  }
-
-  computeInverseEffectiveMass(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    inverseInertiaA: Mat3,
-    inverseInertiaB: Mat3,
-    momentArmA: Vec3,
-    momentArmB: Vec3,
-    direction: Vec3
-  ): number {
-    // #v-ifdef DEV
-    // assert that the direction is normalized within some tolerance
-    if (direction.isNormalized(1e-5) === false) {
-      throw new Error(`expected normalized direction, got ${direction} instead`);
-    }
-    // #v-endif
-
-    // zero out the linear jacobians (to handle static bodies)
-    this.mR1PlusUxAxis.zero();
-    this.mR2xAxis.zero();
-
-    // if not a static body, calculate the linear jacobians
-    if (bodyA.type !== BodyType.static) {
-      this.mR1PlusUxAxis.crossVectors(momentArmA, direction);
-    }
-
-    if (bodyB.type !== BodyType.static) {
-      this.mR2xAxis.crossVectors(momentArmB, direction);
-    }
-
-    // the inverse effective mass is K = J * M^-1 * J_T
-    let inverseEffectiveMass = 0;
-
-    if (bodyA.type === BodyType.dynamic) {
-      this.mInvI1_R1PlusUxAxis.transformVectorFromMat3(this.mR1PlusUxAxis, inverseInertiaA);
-      inverseEffectiveMass += inverseMassA + this.mInvI1_R1PlusUxAxis.dot(this.mR1PlusUxAxis);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      this.mInvI2_R2xAxis.transformVectorFromMat3(this.mR2xAxis, inverseInertiaB);
-      inverseEffectiveMass += inverseMassB + this.mInvI2_R2xAxis.dot(this.mR2xAxis);
-    }
-
-    return inverseEffectiveMass;
-  }
-
-  initialize(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    inverseInertiaA: Mat3,
-    inverseInertiaB: Mat3,
-    momentArmA: Vec3,
-    momentArmB: Vec3,
-    direction: Vec3,
-    bias: number
-  ): void {
-    const inverseEffectiveMass = this.computeInverseEffectiveMass(
-      bodyA,
-      bodyB,
-      inverseMassA,
-      inverseMassB,
-      inverseInertiaA,
-      inverseInertiaB,
-      momentArmA,
-      momentArmB,
-      direction
-    );
-
-    if (inverseEffectiveMass === 0) {
-      this.effectiveMass = 0;
-      this.totalLambda = 0;
-      return;
-    }
-
-    this.effectiveMass = 1 / inverseEffectiveMass;
-    this.bias = bias;
-    this.softness = 0;
-  }
-
-  isActive(): boolean {
-    return this.effectiveMass !== 0;
-  }
-
-  warmStart(bodyA: Body, bodyB: Body, inverseMassA: number, inverseMassB: number, direction: Vec3): void {
-    // this.applyImpulse(bodyA, bodyB, inverseMassA, inverseMassB, direction, this.totalLambda);
-    const deltaLambda = this.totalLambda;
-
-    if (bodyA.type === BodyType.dynamic) {
-      bodyA.linearVelocity.addScaled(direction, -deltaLambda * inverseMassA);
-      bodyA.angularVelocity.addScaled(this.mInvI1_R1PlusUxAxis, -deltaLambda);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      bodyB.linearVelocity.addScaled(direction, +deltaLambda * inverseMassB);
-      bodyB.angularVelocity.addScaled(this.mInvI2_R2xAxis, +deltaLambda);
-    }
-  }
-}
-
-const contactConstraintProps = {
-  normalConstraint: MonomorphType(DirectionalConstraint),
-  tangentConstraint: MonomorphType(DirectionalConstraint),
-  bitangentConstraint: MonomorphType(DirectionalConstraint),
-  localPositionA: MonomorphType(Vec3),
-  localPositionB: MonomorphType(Vec3),
-} as const satisfies PropertyDefinitionMap;
-
-export class ContactConstraint extends createClass<ContactConstraint, typeof contactConstraintProps>(
-  contactConstraintProps
-) {
-  getLambda(out: Vec3, index: number): void {
-    out.x = this.normalConstraint.totalLambda;
-    out.y = this.tangentConstraint.totalLambda;
-    out.z = this.bitangentConstraint.totalLambda;
-  }
-
-  setLambda(value: Vec3): void {
-    this.normalConstraint.totalLambda = value.x;
-    this.tangentConstraint.totalLambda = value.y;
-    this.bitangentConstraint.totalLambda = value.z;
-  }
-
-  copyLambdaFromManifold(manifold: ContactManifold, index: number): void {
-    this.setLambda(manifold.lambdas!.getAtIndex(index)!);
-  }
-
-  warmStart(
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    normal: Vec3,
-    tangent: Vec3,
-    bitangent: Vec3
-  ): void {
-    if (this.tangentConstraint.isActive() || this.bitangentConstraint.isActive()) {
-      this.tangentConstraint.warmStart(bodyA, bodyB, inverseMassA, inverseMassB, tangent);
-      this.bitangentConstraint.warmStart(bodyA, bodyB, inverseMassA, inverseMassB, bitangent);
-    }
-    this.normalConstraint.warmStart(bodyA, bodyB, inverseMassA, inverseMassB, normal);
-  }
-}
-
-const manifoldConstraintProps = {
-  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),
-
-  friction: NumberType(0.0),
-  restitution: NumberType(0.0),
-
-  worldSpaceNormal: MonomorphType(Vec3),
-  worldSpaceTangent: MonomorphType(Vec3),
-  worldSpaceBitangent: MonomorphType(Vec3),
-
-  inverseMassA: NumberType(0.0),
-  inverseMassB: NumberType(0.0),
-
-  contactConstraints: ReferenceListType(ContactConstraint),
-  numContacts: NumberType(0),
-} as const satisfies PropertyDefinitionMap;
-
-/**
- * a contact manifold constraint for one or more points
- */
-export class ManifoldConstraint extends createClass<ManifoldConstraint, typeof manifoldConstraintProps>(
-  manifoldConstraintProps
-) {
-  updateTangentDirections(): void {
-    this.worldSpaceTangent.computeNormalizedPerpendicular(this.worldSpaceNormal);
-    this.worldSpaceBitangent.crossVectors(this.worldSpaceNormal, this.worldSpaceTangent);
-  }
-
-  get key() {
-    return createContactPairKey(this.bodyA!, this.bodyB!);
-  }
-
-  copyLambdasToManifold(manifold: ContactManifold): void {
-    for (let i = 0; i < manifold.numContacts; i++) {
-      this.contactConstraints!.getAtIndex(i)!.getLambda(manifold.lambdas!.getAtIndex(i)!, i);
-    }
-  }
-
-  warmStart(): void {
-    // // recalculate tangents ?
-    this.updateTangentDirections();
-
-    // warm start the contact constraints
-    for (let i = 0; i < this.numContacts; i++) {
-      this.contactConstraints!.getAtIndex(i)!.warmStart(
-        this.bodyA!,
-        this.bodyB!,
-        this.inverseMassA,
-        this.inverseMassB,
-        this.worldSpaceNormal,
-        this.worldSpaceTangent,
-        this.worldSpaceBitangent
-      );
-    }
-  }
-}
-
-export class ManifoldConstraintPool {
-  // the contact manifold pool
-  manifoldConstraintPool: PoolClass<ManifoldConstraint>;
-  contactConstraintPool: PoolClass<ContactConstraint>;
-
-  numMaxManifolds: number;
-  numMaxContacts: number;
-
-  // list of pools for contact constraints
-  // contactConstraintsPoolList: PoolClass<ContactConstraint>[];
-
-  constructor(numMaxManifolds: number, numMaxContacts: number) {
-    this.manifoldConstraintPool = new ManifoldConstraint.Pool(); // numMaxManifolds
-    this.contactConstraintPool = new ContactConstraint.Pool(); // numMaxContacts * numMaxManifolds
-
-    this.numMaxManifolds = numMaxManifolds;
-    this.numMaxContacts = numMaxContacts;
-  }
-
-  createManifoldConstraint(data: InputType<ManifoldConstraint>): ManifoldConstraint {
-    data = data ?? {};
-
-    if (data.contactConstraints) {
-      data.contactConstraints.pool = this.contactConstraintPool;
-    } else {
-      data.contactConstraints = { pool: this.contactConstraintPool };
-    }
-
-    const manifold = ManifoldConstraint.create(data, this.manifoldConstraintPool);
-
-    // todo this behavior probably should change?
-    // currently, we create all the contactConstraints 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.contactConstraints.length >= this.numMaxContacts) {
-      return manifold;
-    }
-
-    for (let i = 0; i < this.numMaxContacts; i++) {
-      manifold.contactConstraints!.create();
-    }
-
-    return manifold;
-  }
-}

package/src/physics/solver/estimateCollisionResponse.ts

@@ -1,146 +0,0 @@
-import { createClass, MonomorphType, PropertyDefinitionMap } from "monomorph";
-import { ContactManifold } from "../manifold/ContactManifold";
-import { Vec3 } from "../../math/vec3";
-import { BodyType } from "../Body";
-import { Mat3 } from "../../math/mat3";
-
-const estimateCollisionResponseResultProps = {
-  deltaLinearVelocityA: MonomorphType(Vec3),
-  deltaAngularVelocityA: MonomorphType(Vec3),
-  deltaLinearVelocityB: MonomorphType(Vec3),
-  deltaAngularVelocityB: MonomorphType(Vec3),
-} as const satisfies PropertyDefinitionMap;
-
-export class EstimateCollisionResponseResult extends createClass<
-  EstimateCollisionResponseResult,
-  typeof estimateCollisionResponseResultProps
->(estimateCollisionResponseResultProps) {
-  reset() {
-    this.deltaLinearVelocityA.zero();
-    this.deltaAngularVelocityA.zero();
-    this.deltaLinearVelocityB.zero();
-    this.deltaAngularVelocityB.zero();
-  }
-}
-
-export function estimateCollisionResponse(
-  result: EstimateCollisionResponseResult,
-  manifold: ContactManifold,
-  timeStepSizeSeconds: number
-) {
-  result.reset();
-  const damping = 0.9;
-  const penetrationResolution = 0.4;
-  const bodyA = manifold.bodyA!;
-  const bodyB = manifold.bodyB!;
-
-  for (let index = 0; index < manifold.numContacts; index++) {
-    manifold.getContactPointA(worldContactPointA, index);
-    worldContactPointA.addVector(manifold.baseTranslation);
-
-    manifold.getContactPointB(worldContactPointB, index);
-    worldContactPointB.addVector(manifold.baseTranslation);
-
-    combinedWorldContactPoint.averageOfVectors(worldContactPointA, worldContactPointB);
-    worldMomentArmA.subtractVectors(combinedWorldContactPoint, bodyA.computedCenterOfMassPosition);
-    worldMomentArmB.subtractVectors(combinedWorldContactPoint, bodyB.computedCenterOfMassPosition);
-
-    bodyA.computeVelocityOfPointRelativeToCenterOfMass(contactVelocityA, worldMomentArmA);
-    bodyB.computeVelocityOfPointRelativeToCenterOfMass(contactVelocityB, worldMomentArmB);
-    contactVelocityAB.subtractVectors(contactVelocityB, contactVelocityA);
-
-    // get the velocity along the contact normal
-    const normalSpeed = contactVelocityAB.dot(manifold.worldSpaceNormal);
-    const deltaSpeed =
-      -normalSpeed * damping - (Math.min(manifold.penetrationDepth, 0) * penetrationResolution) / timeStepSizeSeconds;
-
-    // don't apply impulses if separating
-    if (deltaSpeed < 0) {
-      return;
-    }
-
-    centerOfMassA.copy(bodyA.computedCenterOfMassPosition);
-    centerOfMassB.copy(bodyB.computedCenterOfMassPosition);
-
-    jacobianA.crossVectors(worldMomentArmA, manifold.worldSpaceNormal);
-    jacobianB.crossVectors(worldMomentArmB, manifold.worldSpaceNormal);
-
-    // compute inverse effective mass
-    const direction = manifold.worldSpaceNormal;
-    bodyA.computeInverseInertiaTensor(inverseInertiaA);
-    bodyB.computeInverseInertiaTensor(inverseInertiaB);
-    const inverseMassA = bodyA.inverseMass;
-    const inverseMassB = bodyB.inverseMass;
-
-    // #v-ifdef DEV
-    // assert that the direction is normalized within some tolerance
-    if (direction.isNormalized(1e-5) === false) {
-      throw new Error(`expected normalized direction, got ${direction} instead`);
-    }
-    // #v-endif
-
-    // zero out the linear jacobians (to handle static bodies)
-    mR1PlusUxAxis.zero();
-    mR2xAxis.zero();
-
-    // if not a static body, calculate the linear jacobians
-    if (bodyA.type !== BodyType.static) {
-      mR1PlusUxAxis.crossVectors(worldMomentArmA, direction);
-    }
-
-    if (bodyB.type !== BodyType.static) {
-      mR2xAxis.crossVectors(worldMomentArmB, direction);
-    }
-
-    // the inverse effective mass is K = J * M^-1 * J_T
-    let inverseEffectiveMass = 0;
-
-    if (bodyA.type === BodyType.dynamic) {
-      mInvI1_R1PlusUxAxis.transformVectorFromMat3(mR1PlusUxAxis, inverseInertiaA);
-      inverseEffectiveMass += inverseMassA + mInvI1_R1PlusUxAxis.dot(mR1PlusUxAxis);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      mInvI2_R2xAxis.transformVectorFromMat3(mR2xAxis, inverseInertiaB);
-      inverseEffectiveMass += inverseMassB + mInvI2_R2xAxis.dot(mR2xAxis);
-    }
-
-    // Impulse P = M dv
-    let impulseMagnitude = deltaSpeed / inverseEffectiveMass;
-
-    // Calculate the world space impulse to apply
-    impulse.scaleVector(manifold.worldSpaceNormal, impulseMagnitude);
-
-    const deltaLambda = impulseMagnitude;
-
-    if (bodyA.type === BodyType.dynamic) {
-      result.deltaLinearVelocityA.addScaledToVector(bodyA.linearVelocity, direction, -deltaLambda * inverseMassA);
-      result.deltaAngularVelocityA.addScaledToVector(bodyA.angularVelocity, mInvI1_R1PlusUxAxis, -deltaLambda);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      result.deltaLinearVelocityA.addScaledToVector(bodyB.linearVelocity, direction, +deltaLambda * inverseMassB);
-      result.deltaAngularVelocityB.addScaledToVector(bodyB.angularVelocity, mInvI2_R2xAxis, +deltaLambda);
-    }
-  }
-}
-
-const worldContactPointA = /*@__PURE__*/ Vec3.create();
-const worldContactPointB = /*@__PURE__*/ Vec3.create();
-const combinedWorldContactPoint = /*@__PURE__*/ Vec3.create();
-const worldMomentArmA = /*@__PURE__*/ Vec3.create();
-const worldMomentArmB = /*@__PURE__*/ Vec3.create();
-const contactVelocityA = /*@__PURE__*/ Vec3.create();
-const contactVelocityB = /*@__PURE__*/ Vec3.create();
-const contactVelocityAB = /*@__PURE__*/ Vec3.create();
-const centerOfMassA = /*@__PURE__*/ Vec3.create();
-const centerOfMassB = /*@__PURE__*/ Vec3.create();
-const jacobianA = /*@__PURE__*/ Vec3.create();
-const jacobianB = /*@__PURE__*/ Vec3.create();
-const impulse = /*@__PURE__*/ Vec3.create();
-const mR1PlusUxAxis = /*@__PURE__*/ Vec3.create();
-const mR2xAxis = /*@__PURE__*/ Vec3.create();
-const mInvI1_R1PlusUxAxis = /*@__PURE__*/ Vec3.create();
-const mInvI2_R2xAxis = /*@__PURE__*/ Vec3.create();
-const inverseInertiaA = /*@__PURE__*/ Mat3.create();
-const inverseInertiaB = /*@__PURE__*/ Mat3.create();