@perplexdotgg/bounce 1.0.0

package/src/physics/solver/ManifoldConstraint.ts

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

@@ -0,0 +1,146 @@
+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();