@perplexdotgg/bounce 1.0.0 → 1.0.1

package/src/physics/solver/ContactConstraintModule.ts

@@ -1,659 +0,0 @@
-import { Body, BodyType, ColliderType } from "../Body";
-import { ContactManifold } from "../manifold/ContactManifold";
-import { ContactConstraint, ManifoldConstraint, ManifoldConstraintPool } from "./ManifoldConstraint";
-import { ContactManifoldModule } from "../manifold/ContactManifoldModule";
-import { average, clamp, squared } from "../../math/scalar";
-import { Isometry } from "../../math/isometry";
-import { Mat3 } from "../../math/mat3";
-import { Mat4 } from "../../math/mat4";
-import { createContactPairKey } from "../manifold/ContactManifold";
-import { Vec3 } from "../../math/vec3";
-import { destroyAllInstancesInPool } from "../../helpers";
-import type { World } from "../../world";
-import { WithPool } from "monomorph";
-
-function getCombinedFriction(bodyA: Body, bodyB: Body, fallbackFrictionFunction: CoefficientFunctionType): number {
-  const frictionFunction =
-    bodyA.frictionFunction !== bodyB.frictionFunction
-      ? fallbackFrictionFunction
-      : (bodyA.frictionFunction as keyof typeof coefficientFunctions);
-  return coefficientFunctions[frictionFunction](bodyA.friction, bodyB.friction);
-}
-
-export const enum CoefficientFunctionType {
-  min,
-  max,
-  average,
-}
-
-const coefficientFunctions = {
-  [CoefficientFunctionType.min]: Math.min,
-  [CoefficientFunctionType.max]: Math.max,
-  [CoefficientFunctionType.average]: average,
-} as const;
-
-const contactPointPreserveLambdaMaxDistanceSquared = squared(0.01); // 1 cm
-
-const inverseIsometryB = /*@__PURE__*/ Isometry.create();
-const inverseInertiaTensorA = /*@__PURE__*/ Mat3.create();
-const inverseInertiaTensorB = /*@__PURE__*/ Mat3.create();
-const worldToLocalA = /*@__PURE__*/ Mat4.create();
-const worldToLocalB = /*@__PURE__*/ Mat4.create();
-
-const worldContactPointA = /*@__PURE__*/ Vec3.create();
-const worldContactPointB = /*@__PURE__*/ Vec3.create();
-const oldContactPointA = /*@__PURE__*/ Vec3.create();
-const oldContactPointB = /*@__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 penetrationVector = /*@__PURE__*/ Vec3.create();
-const worldSpaceSurfaceVelocity = /*@__PURE__*/ Vec3.create();
-
-const transformLocalToWorldA = /*@__PURE__*/ Isometry.create();
-const transformLocalToWorldB = /*@__PURE__*/ Isometry.create();
-
-const worldPointA = /*@__PURE__*/ Vec3.create();
-const worldPointB = /*@__PURE__*/ Vec3.create();
-
-const transformAToWorld = /*@__PURE__*/ Mat4.create();
-const transformBToWorld = /*@__PURE__*/ Mat4.create();
-
-const averageWorldContactPoint = /*@__PURE__*/ Vec3.create();
-const momentArmA = /*@__PURE__*/ Vec3.create();
-const momentArmB = /*@__PURE__*/ Vec3.create();
-
-const positionDeltaA = /*@__PURE__*/ Vec3.create();
-const rotationDeltaA = /*@__PURE__*/ Vec3.create();
-const positionDeltaB = /*@__PURE__*/ Vec3.create();
-const rotationDeltaB = /*@__PURE__*/ Vec3.create();
-
-function getCombinedRestitution(
-  bodyA: Body,
-  bodyB: Body,
-  fallbackRestitutionFunction: CoefficientFunctionType
-): number {
-  const restitutionFunction =
-    bodyA.restitutionFunction !== bodyB.restitutionFunction
-      ? fallbackRestitutionFunction
-      : (bodyA.restitutionFunction as keyof typeof coefficientFunctions);
-  return coefficientFunctions[restitutionFunction](bodyA.restitution, bodyB.restitution);
-}
-
-export class ContactConstraintModule {
-  world: World;
-  manifoldConstraints: ManifoldConstraintPool;
-  contactManifoldModule: ContactManifoldModule;
-
-  constructor(world: World, contactManifoldModule: ContactManifoldModule) {
-    this.world = world;
-    this.contactManifoldModule = contactManifoldModule;
-    this.manifoldConstraints = new ManifoldConstraintPool(
-      this.contactManifoldModule.currentManifoldCache.options.maxContactManifolds,
-      4
-    );
-  }
-
-  warmStartConstraints(): void {
-    // warm start the contact constraints
-    for (const constraint of this.manifoldConstraints.manifoldConstraintPool) {
-      constraint.warmStart();
-    }
-  }
-
-  solveFrictionVelocityConstraints(
-    manifoldConstraint: ManifoldConstraint,
-    transformLocalToWorldA: Isometry,
-    transformLocalToWorldB: Isometry
-  ): void {
-    // update the tangents
-    // TODO: is this needed during solving?
-    manifoldConstraint.updateTangentDirections();
-
-    for (let i = 0; i < manifoldConstraint.numContacts; i++) {
-      const contactConstraint = manifoldConstraint.contactConstraints!.getAtIndex(i)!;
-      // solve friction constraints if active
-      if (contactConstraint.tangentConstraint.isActive() || contactConstraint.bitangentConstraint.isActive()) {
-        let tangentLambda = contactConstraint.tangentConstraint.getTotalLambda(
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.worldSpaceTangent
-        );
-        let bitangentLambda = contactConstraint.bitangentConstraint.getTotalLambda(
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.worldSpaceBitangent
-        );
-        const totalLambdaSquared = squared(tangentLambda) + squared(bitangentLambda);
-
-        const maxLambda = manifoldConstraint.friction * contactConstraint.normalConstraint.totalLambda;
-
-        if (totalLambdaSquared > squared(maxLambda)) {
-          const scale = maxLambda / Math.sqrt(totalLambdaSquared);
-          tangentLambda *= scale;
-          bitangentLambda *= scale;
-        }
-
-        worldPointA.transformVectorFromMat4(contactConstraint.localPositionA, transformLocalToWorldA.matrix);
-        worldPointB.transformVectorFromMat4(contactConstraint.localPositionB, transformLocalToWorldB.matrix);
-
-        contactConstraint.tangentConstraint.applyImpulse(
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.inverseMassA,
-          manifoldConstraint.inverseMassB,
-          manifoldConstraint.worldSpaceTangent,
-          tangentLambda
-        );
-
-        contactConstraint.bitangentConstraint.applyImpulse(
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.inverseMassA,
-          manifoldConstraint.inverseMassB,
-          manifoldConstraint.worldSpaceBitangent,
-          bitangentLambda
-        );
-      }
-    }
-  }
-
-  solveNonPenetrationVelocityConstraints(
-    manifoldConstraint: ManifoldConstraint,
-    transformLocalToWorldA: Isometry,
-    transformLocalToWorldB: Isometry
-  ): void {
-    for (let i = 0; i < manifoldConstraint.numContacts; i++) {
-      const contactConstraint = manifoldConstraint.contactConstraints!.getAtIndex(i)!;
-
-      let totalLambda = contactConstraint.normalConstraint.getTotalLambda(
-        manifoldConstraint.bodyA!,
-        manifoldConstraint.bodyB!,
-        manifoldConstraint.worldSpaceNormal
-      );
-
-      totalLambda = clamp(totalLambda, 0, Infinity);
-
-      worldPointA.transformVectorFromMat4(contactConstraint.localPositionA, transformLocalToWorldA.matrix);
-      worldPointB.transformVectorFromMat4(contactConstraint.localPositionB, transformLocalToWorldB.matrix);
-
-      // apply the normal impulse
-      contactConstraint.normalConstraint.applyImpulse(
-        manifoldConstraint.bodyA!,
-        manifoldConstraint.bodyB!,
-        manifoldConstraint.inverseMassA,
-        manifoldConstraint.inverseMassB,
-        manifoldConstraint.worldSpaceNormal,
-        totalLambda
-      );
-    }
-  }
-
-  solveVelocityConstraint(constraint: ManifoldConstraint): void {
-    transformLocalToWorldA.fromRotationAndTranslation(
-      constraint.bodyA!.orientation,
-      constraint.bodyA!.computedCenterOfMassPosition
-    );
-    transformLocalToWorldB.fromRotationAndTranslation(
-      constraint.bodyB!.orientation,
-      constraint.bodyB!.computedCenterOfMassPosition
-    );
-
-    this.solveFrictionVelocityConstraints(constraint, transformLocalToWorldA, transformLocalToWorldB);
-    this.solveNonPenetrationVelocityConstraints(constraint, transformLocalToWorldA, transformLocalToWorldB);
-  }
-
-  solveVelocityConstraintsStep(): void {
-    for (const constraint of this.manifoldConstraints.manifoldConstraintPool) {
-      this.solveVelocityConstraint(constraint);
-    }
-  }
-
-  updateNonPenetrationConstraint(
-    constraint: ContactConstraint,
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    worldContactPointA: Vec3,
-    worldContactPointB: Vec3,
-    worldSpaceNormal: Vec3
-  ): void {
-    averageWorldContactPoint.averageOfVectors(worldContactPointA, worldContactPointB);
-    momentArmA.subtractVectors(averageWorldContactPoint, bodyA.computedCenterOfMassPosition);
-    momentArmB.subtractVectors(averageWorldContactPoint, bodyB.computedCenterOfMassPosition);
-
-    constraint.normalConstraint.initializeWithMassOverride(
-      bodyA,
-      bodyB,
-      inverseMassA,
-      inverseMassB,
-      momentArmA,
-      momentArmB,
-      worldSpaceNormal
-    );
-  }
-
-  solvePositionConstraintWithMassOverride(
-    constraint: ContactConstraint,
-    bodyA: Body,
-    bodyB: Body,
-    inverseMassA: number,
-    inverseMassB: number,
-    direction: Vec3,
-    constraintValue: number,
-    baumgarteValue: number,
-    worldContactPointA: Vec3,
-    worldContactPointB: Vec3
-  ): void {
-    // do nothing if no constraint or spring is active
-    if (constraintValue === 0 || constraint.normalConstraint.isSpringActive()) {
-      return;
-    }
-
-    const lambda = -constraint.normalConstraint.effectiveMass * baumgarteValue * constraintValue;
-
-    if (bodyA.type === BodyType.dynamic) {
-      positionDeltaA.scaleVector(direction, -lambda * inverseMassA);
-      bodyA.world?.broadphase.markDynamicBodyAsDirty(bodyA as WithPool<Body>);
-      bodyA.computedCenterOfMassPosition.addVectors(bodyA.computedCenterOfMassPosition, positionDeltaA);
-
-      rotationDeltaA.scaleVector(constraint.normalConstraint.mInvI1_R1PlusUxAxis, lambda);
-      bodyA.subRotationDelta(rotationDeltaA);
-    }
-
-    if (bodyB.type === BodyType.dynamic) {
-      positionDeltaB.scaleVector(direction, +lambda * inverseMassB);
-      bodyB.world?.broadphase.markDynamicBodyAsDirty(bodyB as WithPool<Body>);
-      bodyB.computedCenterOfMassPosition.addVectors(bodyB.computedCenterOfMassPosition, positionDeltaB);
-
-      rotationDeltaB.scaleVector(constraint.normalConstraint.mInvI2_R2xAxis, lambda);
-      bodyB.addRotationDelta(rotationDeltaB);
-    }
-  }
-
-  solvePositionConstraint(
-    manifoldConstraint: ManifoldConstraint,
-    penetrationSlop: number,
-    maxPenetrationDistance: number,
-    baumgarte: number
-  ): void {
-    // get body transforms
-    transformAToWorld.fromRotationTranslation(
-      manifoldConstraint.bodyA!.orientation,
-      manifoldConstraint.bodyA!.computedCenterOfMassPosition
-    );
-    transformBToWorld.fromRotationTranslation(
-      manifoldConstraint.bodyB!.orientation,
-      manifoldConstraint.bodyB!.computedCenterOfMassPosition
-    );
-
-    // recompute the world contact points from the cached local contact points
-
-    for (let i = 0; i < manifoldConstraint.numContacts; i++) {
-      const contactConstraint = manifoldConstraint.contactConstraints!.getAtIndex(i)!;
-      worldContactPointA.transformVectorFromMat4(contactConstraint.localPositionA, transformAToWorld);
-      worldContactPointB.transformVectorFromMat4(contactConstraint.localPositionB, transformBToWorld);
-
-      penetrationVector.subtractVectors(worldContactPointB, worldContactPointA);
-
-      // negative if penetrated, positive if separated
-      const separation = Math.max(
-        penetrationVector.dot(manifoldConstraint.worldSpaceNormal) + penetrationSlop,
-        -maxPenetrationDistance
-      );
-
-      // only solve if separation is negative (penetrated)
-      if (separation < 0) {
-        this.updateNonPenetrationConstraint(
-          contactConstraint,
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.inverseMassA,
-          manifoldConstraint.inverseMassB,
-          worldContactPointA,
-          worldContactPointB,
-          manifoldConstraint.worldSpaceNormal
-        );
-
-        this.solvePositionConstraintWithMassOverride(
-          contactConstraint,
-          manifoldConstraint.bodyA!,
-          manifoldConstraint.bodyB!,
-          manifoldConstraint.inverseMassA,
-          manifoldConstraint.inverseMassB,
-          manifoldConstraint.worldSpaceNormal,
-          separation,
-          baumgarte,
-          worldContactPointA,
-          worldContactPointB
-        );
-      }
-    }
-  }
-
-  solvePositionConstraintsStep(penetrationSlop: number, maxPenetrationDistance: number, baumgarte: number) {
-    for (const constraint of this.manifoldConstraints.manifoldConstraintPool) {
-      this.solvePositionConstraint(constraint, penetrationSlop, maxPenetrationDistance, baumgarte);
-    }
-  }
-
-  cacheLambdas(): void {
-    // copy the total lambdas from each manifold constraint to the corresponding cached manifold
-    const cache = this.contactManifoldModule.currentManifoldCache;
-
-    for (const constraint of this.manifoldConstraints.manifoldConstraintPool) {
-      const manifold = cache.manifoldMap.get(constraint.key);
-      if (manifold === undefined) {
-        throw new Error(`expected to find manifold ${constraint.key} in cache`);
-      }
-
-      constraint.copyLambdasToManifold(manifold);
-    }
-  }
-
-  clearContactConstraints(): void {
-    destroyAllInstancesInPool(this.manifoldConstraints.manifoldConstraintPool);
-  }
-
-  addContactConstraints(
-    bodyA: Body,
-    bodyB: Body,
-    manifold: ContactManifold,
-    timeStepSizeSeconds: number,
-    isWarmStartingEnabled: boolean,
-    minVelocityForElasticContact: number
-  ) {
-    // ensure the world space normal is normalized
-    manifold.worldSpaceNormal.normalize();
-    // console.log("adding contact constraint", manifold.worldSpaceNormal.toObject());
-
-    // #v-ifdef DEV
-    // assert that the direction is normalized within some tolerance
-    if (manifold.worldSpaceNormal.isNormalized(1e-5) === false) {
-      throw new Error(`expected normalized manifold normal, got ${manifold.worldSpaceNormal} instead`);
-    }
-    // #v-endif
-
-    const cachedManifold = this.contactManifoldModule.currentManifoldCache.manifolds.createContactManifold({
-      bodyA: bodyA,
-      bodyB: bodyB,
-      worldSpaceNormal: manifold.worldSpaceNormal,
-      firstWorldSpaceNormal: manifold.firstWorldSpaceNormal,
-      penetrationDepth: manifold.penetrationDepth,
-      nextContactManifold: manifold.nextContactManifold,
-      numContacts: manifold.numContacts,
-      subShapeIdA: manifold.subShapeIdA,
-      subShapeIdB: manifold.subShapeIdB,
-    });
-
-    this.contactManifoldModule.currentManifoldCache.manifoldMap.set(cachedManifold.key, cachedManifold);
-
-    // for the cached manifold, we store the worldSpaceNormal in the space of bodyB
-
-    inverseIsometryB.fromInverseRotationAndTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
-    inverseIsometryB.matrix.multiply3x3(cachedManifold.worldSpaceNormal, manifold.worldSpaceNormal);
-    cachedManifold.worldSpaceNormal.normalize();
-
-    const previousManifold = this.contactManifoldModule.previousManifoldCache.manifoldMap.get(cachedManifold.key);
-    // check if the manifold exists in the previous cache
-    if (previousManifold !== undefined) {
-      // persistent contact
-      // TODO: get the previous lambdas per constraint and use them to warm start the constraints
-      cachedManifold.copyLambdas(previousManifold);
-    } else {
-      // new contact
-    }
-
-    // no constraints if either body does not resolve contacts
-    if (bodyA.colliderType !== ColliderType.resolveContact && bodyB.colliderType !== ColliderType.resolveContact) {
-      return;
-    }
-
-    // no constraints if neither body is dynamic
-    if (bodyA.type !== BodyType.dynamic && bodyB.type !== BodyType.dynamic) {
-      return;
-    }
-
-    const manifoldConstraint = this.manifoldConstraints.createManifoldConstraint({
-      bodyA: bodyA,
-      bodyB: bodyB,
-      subShapeIdA: manifold.subShapeIdA,
-      subShapeIdB: manifold.subShapeIdB,
-      friction: getCombinedFriction(bodyA, bodyB, CoefficientFunctionType.average),
-      restitution: getCombinedRestitution(bodyA, bodyB, CoefficientFunctionType.average),
-      inverseMassA: bodyA.inverseMass,
-      inverseMassB: bodyB.inverseMass,
-      numContacts: 0,
-    });
-
-    // TODO: which normal should we use, A or B?
-    manifoldConstraint.worldSpaceNormal.normalizeVector(manifold.worldSpaceNormal);
-    manifoldConstraint.updateTangentDirections();
-    manifoldConstraint.numContacts = manifold.numContacts;
-
-    // get mass properties
-
-    bodyA.computeInverseInertiaTensor(inverseInertiaTensorA);
-    bodyB.computeInverseInertiaTensor(inverseInertiaTensorB);
-
-    // get the world to local transform for each body
-
-    worldToLocalA.fromInverseRotationAndTranslation(bodyA.orientation, bodyA.computedCenterOfMassPosition);
-    worldToLocalB.fromInverseRotationAndTranslation(bodyB.orientation, bodyB.computedCenterOfMassPosition);
-
-    // iterate over the contacts and add the constraints
-    for (let i = 0; i < manifold.numContacts; i++) {
-      this.addContactConstraint(
-        bodyA,
-        bodyB,
-        manifold,
-        manifoldConstraint,
-        inverseInertiaTensorA,
-        inverseInertiaTensorB,
-        worldToLocalA,
-        worldToLocalB,
-        cachedManifold,
-        i,
-        timeStepSizeSeconds,
-        isWarmStartingEnabled,
-        minVelocityForElasticContact
-      );
-    }
-
-    // set the new cached manifold as the head of the current manifold cache
-    const contactPairKey = createContactPairKey(manifold.bodyA!, manifold.bodyB!);
-    const contactPair = this.contactManifoldModule.currentManifoldCache.pairMap.get(contactPairKey);
-
-    if (contactPair === undefined) {
-      throw new Error("contact pair not found");
-    }
-
-    // TODO: implement manifold list
-    const currentFirstManifold = contactPair.firstContactManifold;
-    if (currentFirstManifold === null) {
-      // case: no current first manifold, simply set the new manifold as the first
-      contactPair.firstContactManifold = cachedManifold;
-    } else {
-      // case: there is a current first manifold, set the current first as the next of the new manifold, and set the new manifold as the new first
-      cachedManifold.nextContactManifold = currentFirstManifold;
-      contactPair.firstContactManifold = cachedManifold;
-    }
-
-    contactPair.firstContactManifold = cachedManifold;
-
-    // reset the current cached manifold's lambdas
-    cachedManifold.resetLambdas();
-  }
-
-  addContactConstraint(
-    bodyA: Body,
-    bodyB: Body,
-    manifold: ContactManifold,
-    manifoldConstraint: ManifoldConstraint,
-    inverseInertiaTensorA: Mat3,
-    inverseInertiaTensorB: Mat3,
-    worldToLocalA: Mat4,
-    worldToLocalB: Mat4,
-    cachedManifold: ContactManifold,
-    index: number,
-    timeStepSizeSeconds: number,
-    isWarmStartingEnabled: boolean,
-    minVelocityForElasticContact: number
-  ): void {
-    const contactConstraint = manifoldConstraint.contactConstraints!.getAtIndex(index)!;
-    // init the constraint directions
-
-    // init the constraint local contact points
-
-    manifold.getContactPointA(worldContactPointA, index);
-    worldContactPointA.addVector(manifold.baseTranslation);
-    manifold.getContactPointB(worldContactPointB, index);
-    worldContactPointB.addVector(manifold.baseTranslation);
-
-    contactConstraint.localPositionA.transformVectorFromMat4(worldContactPointA, worldToLocalA);
-    contactConstraint.localPositionB.transformVectorFromMat4(worldContactPointB, worldToLocalB);
-
-    // init the lambdas to zero
-    contactConstraint.normalConstraint.totalLambda = 0;
-    contactConstraint.tangentConstraint.totalLambda = 0;
-    contactConstraint.bitangentConstraint.totalLambda = 0;
-
-    // check if we have a close contact point from last update
-    const oldManifold = this.contactManifoldModule.previousManifoldCache.manifoldMap.get(manifold.key);
-    if (oldManifold !== undefined) {
-      // check if any of the contact points are close enough to be considered the same contact
-
-      for (let i = 0; i < oldManifold.numContacts; i++) {
-        oldManifold.getContactPointA(oldContactPointA, index);
-        oldManifold.getContactPointB(oldContactPointB, index);
-
-        if (
-          contactConstraint.localPositionA.isClose(oldContactPointA, contactPointPreserveLambdaMaxDistanceSquared) &&
-          contactConstraint.localPositionB.isClose(oldContactPointB, contactPointPreserveLambdaMaxDistanceSquared)
-        ) {
-          if (isWarmStartingEnabled) {
-            contactConstraint.copyLambdaFromManifold(oldManifold, index);
-          }
-          break;
-        }
-      }
-    }
-
-    // store the contact points for next frame
-    cachedManifold.setContactPointA(contactConstraint.localPositionA, index);
-    cachedManifold.setContactPointB(contactConstraint.localPositionB, index);
-
-    // 1. init the velocity constraint
-    // wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(deltaTime, bodyA, bodyB, inverseMassA, inverseMassB, inverseInertiaA, inverseInertiaB,
-    // worldContactPointA, worldContactPointB, worldSpaceNormal, worldSpaceTangent, worldSpaceBitangent, settings, mPhysicsSettings.mMinVelocityForRestitution);
-
-    // init the world moment arms
-
-    combinedWorldContactPoint.averageOfVectors(worldContactPointA, worldContactPointB);
-    worldMomentArmA.subtractVectors(combinedWorldContactPoint, bodyA.computedCenterOfMassPosition);
-    worldMomentArmB.subtractVectors(combinedWorldContactPoint, bodyB.computedCenterOfMassPosition);
-
-    // init the contact point velocities
-
-    bodyA.computeVelocityOfPointRelativeToCenterOfMass(contactVelocityA, worldMomentArmA);
-    bodyB.computeVelocityOfPointRelativeToCenterOfMass(contactVelocityB, worldMomentArmB);
-    contactVelocityAB.subtractVectors(contactVelocityB, contactVelocityA);
-
-    // init the relative contact point speed along the normal
-    const normalSpeed = contactVelocityAB.dot(manifoldConstraint.worldSpaceNormal);
-
-    // init the contact penetration along the normal (positive -> penetrated, negative -> separated)
-
-    penetrationVector.subtractVectors(worldContactPointA, worldContactPointB);
-    const penetration = penetrationVector.dot(manifoldConstraint.worldSpaceNormal);
-
-    // #v-ifdef DEV
-    // assert that penetration is positive, TODO: handle negative penetration (speculative contacts)
-    if (penetration < 0) {
-      // throw new Error("got here without being in contact, handling of speculative contacts not implemented yet");
-    }
-    // #v-endif
-
-    // init the speculative contact velocity bias
-    const speculativeContactVelocityBias = Math.max(0, -penetration / timeStepSizeSeconds);
-
-    // init velocity bias (handles restitution)
-    let normalVelocityBias: number;
-
-    const isVelocityBigEnoughForRestitution =
-      manifoldConstraint.restitution > 0 &&
-      normalSpeed < -minVelocityForElasticContact &&
-      normalSpeed < -speculativeContactVelocityBias;
-
-    if (isVelocityBigEnoughForRestitution) {
-      if (normalSpeed < -speculativeContactVelocityBias) {
-        normalVelocityBias = manifoldConstraint.restitution * normalSpeed;
-      } else {
-        normalVelocityBias = speculativeContactVelocityBias;
-      }
-    } else {
-      normalVelocityBias = speculativeContactVelocityBias;
-    }
-
-    // init the normal constraint
-    contactConstraint.normalConstraint.initialize(
-      bodyA,
-      bodyB,
-      manifoldConstraint.inverseMassA,
-      manifoldConstraint.inverseMassB,
-      inverseInertiaTensorA,
-      inverseInertiaTensorB,
-      worldMomentArmA,
-      worldMomentArmB,
-      manifoldConstraint.worldSpaceNormal,
-      normalVelocityBias
-    );
-
-    // init the tangent constraints
-    if (manifoldConstraint.friction === 0) {
-      contactConstraint.tangentConstraint.effectiveMass = 0;
-      contactConstraint.tangentConstraint.totalLambda = 0;
-
-      contactConstraint.bitangentConstraint.effectiveMass = 0;
-      contactConstraint.bitangentConstraint.totalLambda = 0;
-      return;
-    }
-
-    // get the surface velocity relative to the tangents
-    // TODO: this world space velocity is a placeholder for something that a user can pass in through settings
-
-    worldSpaceSurfaceVelocity.zero();
-    const tangentSpeed = worldSpaceSurfaceVelocity.dot(manifoldConstraint.worldSpaceTangent);
-    const bitangentSpeed = worldSpaceSurfaceVelocity.dot(manifoldConstraint.worldSpaceBitangent);
-
-    // init the tangent constraints
-    contactConstraint.tangentConstraint.initialize(
-      bodyA,
-      bodyB,
-      manifoldConstraint.inverseMassA,
-      manifoldConstraint.inverseMassB,
-      inverseInertiaTensorA,
-      inverseInertiaTensorB,
-      worldMomentArmA,
-      worldMomentArmB,
-      manifoldConstraint.worldSpaceTangent,
-      tangentSpeed
-    );
-    contactConstraint.bitangentConstraint.initialize(
-      bodyA,
-      bodyB,
-      manifoldConstraint.inverseMassA,
-      manifoldConstraint.inverseMassB,
-      inverseInertiaTensorA,
-      inverseInertiaTensorB,
-      worldMomentArmA,
-      worldMomentArmB,
-      manifoldConstraint.worldSpaceBitangent,
-      bitangentSpeed
-    );
-  }
-}