@perplexdotgg/bounce 1.0.0

package/src/physics/solver/ContactConstraintModule.ts

@@ -0,0 +1,659 @@
+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
+    );
+  }
+}