npm - @perplexdotgg/bounce - Versions diffs - 1.0.0 - Mend

@perplexdotgg/bounce 1.0.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.

Files changed (140) hide show

package/LICENSE +21 -0
package/README.md +30 -0
package/package.json +32 -0
package/src/builders/ConvexHullBuilder.ts +437 -0
package/src/builders/ConvexHullBuilder2d.ts +344 -0
package/src/builders/ConvexHullBuilder3d.ts +1689 -0
package/src/builders/HeightMapBuilder.ts +414 -0
package/src/builders/TriangleMeshBuilder.ts +92 -0
package/src/collision/CastShapesModule.ts +184 -0
package/src/collision/CollideShapesModule.ts +152 -0
package/src/collision/HeightMapCaster.ts +38 -0
package/src/collision/HeightMapCollider.ts +33 -0
package/src/collision/TriangleCaster.ts +249 -0
package/src/collision/TriangleCollider.ts +308 -0
package/src/collision/TriangleCollider2.ts +379 -0
package/src/collision/activeEdge.ts +146 -0
package/src/collision/cast/cast.ts +139 -0
package/src/collision/cast/castCompoundVsCompound.ts +59 -0
package/src/collision/cast/castCompoundVsConvex.ts +116 -0
package/src/collision/cast/castConvexVsCompound.ts +123 -0
package/src/collision/cast/castConvexVsConvex.ts +213 -0
package/src/collision/cast/castConvexVsHeightMap.ts +73 -0
package/src/collision/cast/castConvexVsTriangleMesh.ts +56 -0
package/src/collision/cast/castRayVsCompound.ts +44 -0
package/src/collision/cast/castRayVsConvex.ts +45 -0
package/src/collision/cast/castRayVsHeightMap.ts +58 -0
package/src/collision/cast/castRayVsTriangleMesh.ts +58 -0
package/src/collision/closestPoints/closestPoints.ts +23 -0
package/src/collision/closestPoints/computeBarycentricCoordinates2d.ts +32 -0
package/src/collision/closestPoints/computeBarycentricCoordinates3d.ts +81 -0
package/src/collision/closestPoints/computeClosestPointOnLine.ts +30 -0
package/src/collision/closestPoints/computeClosestPointOnTetrahedron.ts +96 -0
package/src/collision/closestPoints/computeClosestPointOnTriangle.ts +195 -0
package/src/collision/closestPoints/isOriginOutsideOfPlane.ts +25 -0
package/src/collision/closestPoints/isOriginOutsideOfTrianglePlanes.ts +72 -0
package/src/collision/collide/collide.ts +146 -0
package/src/collision/collide/collideCompoundVsCompound.ts +60 -0
package/src/collision/collide/collideCompoundVsConvex.ts +59 -0
package/src/collision/collide/collideCompoundVsHeightMap.ts +73 -0
package/src/collision/collide/collideCompoundVsTriangleMesh.ts +56 -0
package/src/collision/collide/collideConvexVsCompound.ts +57 -0
package/src/collision/collide/collideConvexVsConvex.ts +225 -0
package/src/collision/collide/collideConvexVsConvexImp.ts +236 -0
package/src/collision/collide/collideConvexVsHeightMap.ts +53 -0
package/src/collision/collide/collideConvexVsTriangleMesh.ts +58 -0
package/src/collision/collide/collideHeightMapVsCompound.ts +69 -0
package/src/collision/collide/collideHeightMapVsConvex.ts +53 -0
package/src/collision/collide/collideSphereVsSphere.ts +81 -0
package/src/collision/collide/collideTriangleMeshVsCompound.ts +58 -0
package/src/collision/collide/collideTriangleMeshVsConvex.ts +58 -0
package/src/collision/epa/EpaConvexHullBuilder.ts +397 -0
package/src/collision/epa/StaticArray.ts +154 -0
package/src/collision/epa/TriangleFactory.ts +32 -0
package/src/collision/epa/arrays.ts +99 -0
package/src/collision/epa/binaryHeap.ts +82 -0
package/src/collision/epa/structs.ts +227 -0
package/src/collision/gjk/GjkModule.ts +864 -0
package/src/collision/gjk/PenetrationDepthModule.ts +493 -0
package/src/collision/gjk/SupportPoints.ts +50 -0
package/src/collision/imp/MinkowskiDifference.ts +36 -0
package/src/collision/imp/computeExploredDistanceLowerUpperBound.ts +40 -0
package/src/collision/imp/finalizeImpResult.ts +69 -0
package/src/collision/imp/findContactImp.ts +196 -0
package/src/collision/imp/imp.ts +28 -0
package/src/collision/imp/incrementalMinimumDistanceExploreDirection.ts +207 -0
package/src/collision/mpr/findPortal.ts +152 -0
package/src/collision/mpr/mpr.ts +29 -0
package/src/collision/mpr/updatePortal.ts +52 -0
package/src/constraints/BaseConstraint.ts +50 -0
package/src/constraints/ConstraintOptions.ts +22 -0
package/src/constraints/ConstraintSolver.ts +119 -0
package/src/constraints/DistanceConstraint.ts +229 -0
package/src/constraints/FixedConstraint.ts +203 -0
package/src/constraints/HingeConstraint.ts +460 -0
package/src/constraints/PointConstraint.ts +108 -0
package/src/constraints/components/AngleComponent.ts +226 -0
package/src/constraints/components/AxisComponent.ts +263 -0
package/src/constraints/components/HingeComponent.ts +215 -0
package/src/constraints/components/Motor.ts +36 -0
package/src/constraints/components/PointConstraintComponent.ts +179 -0
package/src/constraints/components/RotationEulerComponent.ts +139 -0
package/src/constraints/components/Spring.ts +30 -0
package/src/constraints/components/SpringComponent.ts +71 -0
package/src/constraints/types.ts +6 -0
package/src/helpers.ts +147 -0
package/src/index.ts +50 -0
package/src/math/BasicTransform.ts +19 -0
package/src/math/NumberValue.ts +13 -0
package/src/math/isometry.ts +64 -0
package/src/math/mat3.ts +529 -0
package/src/math/mat4.ts +588 -0
package/src/math/quat.ts +193 -0
package/src/math/scalar.ts +81 -0
package/src/math/tensor.ts +17 -0
package/src/math/vec3.ts +589 -0
package/src/math/vec4.ts +10 -0
package/src/physics/Body.ts +581 -0
package/src/physics/CollisionFilter.ts +52 -0
package/src/physics/SleepModule.ts +163 -0
package/src/physics/broadphase/BodyPairsModule.ts +363 -0
package/src/physics/broadphase/BvhModule.ts +237 -0
package/src/physics/broadphase/BvhTree.ts +803 -0
package/src/physics/broadphase/ConstraintPairsModule.ts +385 -0
package/src/physics/broadphase/TriangleMeshBvhTree.ts +379 -0
package/src/physics/manifold/ContactManifold.ts +227 -0
package/src/physics/manifold/ContactManifoldModule.ts +623 -0
package/src/physics/manifold/Face.ts +119 -0
package/src/physics/manifold/ManifoldCache.ts +116 -0
package/src/physics/manifold/clipping/clipPolyVsEdge.ts +131 -0
package/src/physics/manifold/clipping/clipPolyVsPlane.ts +73 -0
package/src/physics/manifold/clipping/clipPolyVsPoly.ts +72 -0
package/src/physics/narrowphase/CollideBodiesModule.ts +755 -0
package/src/physics/solver/ContactConstraintModule.ts +659 -0
package/src/physics/solver/ManifoldConstraint.ts +420 -0
package/src/physics/solver/estimateCollisionResponse.ts +146 -0
package/src/shape/Aabb.ts +400 -0
package/src/shape/Box.ts +231 -0
package/src/shape/Capsule.ts +332 -0
package/src/shape/CompoundShape.ts +288 -0
package/src/shape/Convex.ts +130 -0
package/src/shape/ConvexHull.ts +423 -0
package/src/shape/Cylinder.ts +313 -0
package/src/shape/HeightMap.ts +511 -0
package/src/shape/Line.ts +14 -0
package/src/shape/Plane.ts +116 -0
package/src/shape/Ray.ts +81 -0
package/src/shape/Segment.ts +25 -0
package/src/shape/Shape.ts +77 -0
package/src/shape/Sphere.ts +181 -0
package/src/shape/TransformedShape.ts +51 -0
package/src/shape/Triangle.ts +122 -0
package/src/shape/TriangleMesh.ts +186 -0
package/src/types.ts +1 -0
package/src/world.ts +1335 -0
package/tests/BodyPairsModule.test.ts +71 -0
package/tests/BvhTree.test.ts +406 -0
package/tests/test.md +642 -0
package/tests/vec3.test.ts +12 -0
package/tsconfig.json +20 -0
package/vite.config.js +40 -0

package/src/constraints/components/AngleComponent.ts ADDED Viewed

@@ -0,0 +1,226 @@
+import {
+  createClass,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { Body, BodyType } from "../../physics/Body";
+import { ConstraintOptions } from "../ConstraintOptions";
+import { SpringComponent } from "./SpringComponent";
+import { Vec3 } from "../../math/vec3";
+import type { World } from "../../world";
+import { clamp } from "../../math/scalar";
+import { Spring, SpringMode } from "./Spring";
+const spinBA = /*@__PURE__*/ Vec3.create();
+const deltaSpin = /*@__PURE__*/ Vec3.create();
+const inverseEffectiveMass = /*@__PURE__*/ Vec3.create();
+export const angleComponentProps = {
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  options: MonomorphType(ConstraintOptions, {
+    positionBaumgarte: 0.8,
+    velocityBaumgarte: 1.0,
+    strength: 1.0,
+  }),
+  springConstraintPart: MonomorphType(SpringComponent, undefined, true),
+  effectiveMass: NumberType(0, true),
+  effectiveInverseInertiaA: MonomorphType(Vec3, undefined, true),
+  effectiveInverseInertiaB: MonomorphType(Vec3, undefined, true),
+  totalLambda: NumberType(0, true),
+} as const satisfies PropertyDefinitionMap;
+export class AngleComponent extends createClass<AngleComponent, typeof angleComponentProps>(angleComponentProps) {
+  declare world: World;
+  deactivate(): void {
+    this.effectiveMass = 0;
+    this.totalLambda = 0;
+  }
+  isActive(): boolean {
+    return this.effectiveMass !== 0;
+  }
+  applyDirectImpulse(bodyA: Body, bodyB: Body, impulse: number): void {
+    if (impulse === 0) {
+      return;
+    }
+    if (bodyA.type === BodyType.dynamic) {
+      bodyA.angularVelocity.subScaledVector(this.effectiveInverseInertiaA, impulse);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      bodyB.angularVelocity.addScaledVector(this.effectiveInverseInertiaB, impulse);
+    }
+  }
+  warmStart(bodyA: Body, bodyB: Body, warmStartImpulseRatio: number): void {
+    this.totalLambda *= warmStartImpulseRatio;
+    this.applyDirectImpulse(bodyA, bodyB, this.totalLambda);
+  }
+  solveVelocity(bodyA: Body, bodyB: Body, worldSpaceAxis: Vec3, minLambda: number, maxLambda: number): void {
+    spinBA.subtractVectors(bodyA.angularVelocity, bodyB.angularVelocity);
+    let lambda =
+      this.effectiveMass * (worldSpaceAxis.dot(spinBA) - this.springConstraintPart.getBias(this.totalLambda));
+    lambda *= this.options.velocityBaumgarte;
+    const newLambda = clamp(this.totalLambda + lambda, minLambda, maxLambda);
+    lambda = newLambda - this.totalLambda;
+    this.totalLambda = newLambda;
+    this.applyDirectImpulse(bodyA, bodyB, lambda);
+  }
+  solvePosition(bodyA: Body, bodyB: Body, constraint: number): void {
+    if (constraint === 0 || this.springConstraintPart.isActive()) {
+      return;
+    }
+    const lambda = -this.effectiveMass * constraint * this.options.positionBaumgarte;
+    if (bodyA.type === BodyType.dynamic) {
+      deltaSpin.copy(this.effectiveInverseInertiaA).scale(lambda);
+      bodyA.subRotationDelta(deltaSpin);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      deltaSpin.copy(this.effectiveInverseInertiaB).scale(lambda);
+      bodyB.addRotationDelta(deltaSpin);
+    }
+  }
+  setupInverseEffectiveMass(bodyA: Body, bodyB: Body, axis: Vec3): number {
+    // TODO: assert axis is normalized?
+    if (bodyA.type === BodyType.dynamic) {
+      bodyA.computeEffectiveInverseInertiaVector(this.effectiveInverseInertiaA, bodyA.orientation, axis);
+    } else {
+      this.effectiveInverseInertiaA.zero();
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      bodyB.computeEffectiveInverseInertiaVector(this.effectiveInverseInertiaB, bodyB.orientation, axis);
+    } else {
+      this.effectiveInverseInertiaB.zero();
+    }
+    inverseEffectiveMass.addVectors(this.effectiveInverseInertiaA, this.effectiveInverseInertiaB);
+    return axis.dot(inverseEffectiveMass);
+  }
+  setup(bodyA: Body, bodyB: Body, worldSpaceAxis: Vec3, bias: number = 0.0): void {
+    const inverseEffectiveMass = this.setupInverseEffectiveMass(bodyA, bodyB, worldSpaceAxis);
+    if (inverseEffectiveMass === 0) {
+      this.deactivate();
+    } else {
+      this.effectiveMass = 1 / inverseEffectiveMass;
+      this.springConstraintPart.setupSpringPropertiesWithBias(bias);
+    }
+    this.effectiveMass *= this.options.strength;
+  }
+  setupWithFrequency(
+    bodyA: Body,
+    bodyB: Body,
+    worldSpaceAxis: Vec3,
+    bias: number,
+    constraint: number,
+    frequency: number,
+    damping: number,
+    deltaTime: number
+  ): void {
+    const inverseEffectiveMass = this.setupInverseEffectiveMass(bodyA, bodyB, worldSpaceAxis);
+    if (inverseEffectiveMass === 0) {
+      this.deactivate();
+    } else {
+      this.effectiveMass = this.springConstraintPart.setupWithFrequency(
+        deltaTime,
+        inverseEffectiveMass,
+        bias,
+        constraint,
+        frequency,
+        damping
+      );
+    }
+  }
+  setupWithStiffness(
+    bodyA: Body,
+    bodyB: Body,
+    worldSpaceAxis: Vec3,
+    bias: number,
+    constraint: number,
+    stiffness: number,
+    damping: number,
+    deltaTime: number
+  ): void {
+    const inverseEffectiveMass = this.setupInverseEffectiveMass(bodyA, bodyB, worldSpaceAxis);
+    if (inverseEffectiveMass === 0) {
+      this.deactivate();
+    } else {
+      this.effectiveMass = this.springConstraintPart.setupWithStiffness(
+        deltaTime,
+        inverseEffectiveMass,
+        bias,
+        constraint,
+        stiffness,
+        damping
+      );
+    }
+  }
+  setupWithSpring(
+    bodyA: Body,
+    bodyB: Body,
+    worldSpaceAxis: Vec3,
+    bias: number,
+    constraint: number,
+    spring: Spring,
+    deltaTime: number
+  ): void {
+    const inverseEffectiveMass = this.setupInverseEffectiveMass(bodyA, bodyB, worldSpaceAxis);
+    if (inverseEffectiveMass === 0) {
+      this.deactivate();
+      return;
+    }
+    switch (spring.mode) {
+      case SpringMode.UseFrequency: {
+        this.effectiveMass = this.springConstraintPart.setupWithFrequency(
+          deltaTime,
+          inverseEffectiveMass,
+          bias,
+          constraint,
+          spring.frequency,
+          spring.damping
+        );
+        break;
+      }
+      case SpringMode.UseStiffness: {
+        this.effectiveMass = this.springConstraintPart.setupWithStiffness(
+          deltaTime,
+          inverseEffectiveMass,
+          bias,
+          constraint,
+          spring.stiffness,
+          spring.damping
+        );
+        break;
+      }
+      default: {
+        throw new Error("Invalid spring mode");
+      }
+    }
+  }
+}

package/src/constraints/components/AxisComponent.ts ADDED Viewed

@@ -0,0 +1,263 @@
+import {
+  createClass,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { ConstraintOptions } from "../ConstraintOptions";
+import { Body, BodyType } from "../../physics/Body";
+import { Vec3 } from "../../math/vec3";
+import { SpringComponent } from "./SpringComponent";
+import type { World } from "../../world";
+import { Mat3 } from "../../math/mat3";
+import { Spring, SpringMode } from "./Spring";
+import { clamp } from "../../math/scalar";
+const linearVelocityBA = /*@__PURE__*/ Vec3.create();
+const inverseInertiaA = /*@__PURE__*/ Mat3.create();
+const inverseInertiaB = /*@__PURE__*/ Mat3.create();
+const linearImpulse = /*@__PURE__*/ Vec3.create();
+const angularImpulse = /*@__PURE__*/ Vec3.create();
+export const axisComponentProps = {
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  options: MonomorphType(ConstraintOptions, {
+    positionBaumgarte: 0.8,
+    velocityBaumgarte: 1.0,
+    strength: 1.0,
+  }),
+  mR1PlusUxAxis: MonomorphType(Vec3, { x: 0, y: 0, z: 0 }),
+  mR2xAxis: MonomorphType(Vec3, { x: 0, y: 0, z: 0 }),
+  mInvI1_R1PlusUxAxis: MonomorphType(Vec3, { x: 0, y: 0, z: 0 }),
+  mInvI2_R2xAxis: MonomorphType(Vec3, { x: 0, y: 0, z: 0 }),
+  effectiveMass: NumberType(0),
+  totalLambda: NumberType(0),
+  springComponent: MonomorphType(SpringComponent),
+} as const satisfies PropertyDefinitionMap;
+export class AxisComponent extends createClass<AxisComponent, typeof axisComponentProps>(axisComponentProps) {
+  declare world: World;
+  deactivate(): void {
+    this.effectiveMass = 0;
+    this.totalLambda = 0;
+  }
+  isActive(): boolean {
+    return this.effectiveMass > 0;
+  }
+  computeInverseEffectiveMass(
+    inverseMassA: number,
+    inverseMassB: number,
+    inverseInertiaA: Mat3,
+    inverseInertiaB: Mat3,
+    momentArmA: Vec3,
+    momentArmB: Vec3,
+    direction: Vec3
+  ): number {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    // #v-ifdef DEV
+    // assert that the direction is normalized within some tolerance
+    if (direction.isNormalized(1e-5) === false) {
+      throw new Error("direction must be normalized");
+    }
+    // #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;
+  }
+  // TODO: this is copied from DirectionalConstraint, should be refactored
+  getTotalLambda(direction: Vec3): number {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    // #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
+    linearVelocityBA.subtractVectors(bodyA.linearVelocity, bodyB.linearVelocity);
+    jv += direction.dot(linearVelocityBA);
+    // jacobian multiplied by the angular relative velocity
+    if (bodyA.type !== BodyType.static) {
+      jv += this.mR1PlusUxAxis.dot(bodyA.angularVelocity);
+    }
+    if (bodyB.type !== BodyType.static) {
+      jv -= this.mR2xAxis.dot(bodyB.angularVelocity);
+    }
+    const springBias = this.springComponent.getBias(this.totalLambda);
+    const lambda = this.effectiveMass * (jv - springBias);
+    const accumulatedLambda = this.totalLambda + lambda;
+    return accumulatedLambda;
+  }
+  applyDirectImpulse(direction: Vec3, lambda: number): void {
+    if (lambda === 0) {
+      return;
+    }
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    if (bodyA.type === BodyType.dynamic) {
+      linearImpulse.scaleVector(direction, lambda * bodyA.inverseMass);
+      angularImpulse.scaleVector(this.mInvI1_R1PlusUxAxis, lambda);
+      bodyA.linearVelocity.subtractVectors(bodyA.linearVelocity, linearImpulse);
+      bodyA.angularVelocity.subtractVectors(bodyA.angularVelocity, angularImpulse);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      linearImpulse.scaleVector(direction, lambda * bodyB.inverseMass);
+      angularImpulse.scaleVector(this.mInvI2_R2xAxis, lambda);
+      bodyB.linearVelocity.addVectors(bodyB.linearVelocity, linearImpulse);
+      bodyB.angularVelocity.addVectors(bodyB.angularVelocity, angularImpulse);
+    }
+  }
+  setup(deltaTime: number, armA: Vec3, armB: Vec3, direction: Vec3, bias: number, error: number, spring: Spring): void {
+    // calculate effective mass
+    const inverseMassA = this.bodyA!.inverseMass;
+    const inverseMassB = this.bodyB!.inverseMass;
+    this.bodyA!.computeInverseInertiaTensor(inverseInertiaA);
+    this.bodyB!.computeInverseInertiaTensor(inverseInertiaB);
+    const inverseEffectiveMass = this.computeInverseEffectiveMass(
+      inverseMassA,
+      inverseMassB,
+      inverseInertiaA,
+      inverseInertiaB,
+      armA,
+      armB,
+      direction
+    );
+    switch (true) {
+      case inverseEffectiveMass === 0: {
+        this.deactivate();
+        break;
+      }
+      case spring.mode === SpringMode.UseFrequency: {
+        this.effectiveMass = this.springComponent.setupWithFrequency(
+          deltaTime,
+          inverseEffectiveMass,
+          bias,
+          error,
+          spring.frequency,
+          spring.damping
+        );
+        break;
+      }
+      case spring.mode === SpringMode.UseStiffness: {
+        this.effectiveMass = this.springComponent.setupWithStiffness(
+          deltaTime,
+          inverseEffectiveMass,
+          bias,
+          error,
+          spring.stiffness,
+          spring.damping
+        );
+        break;
+      }
+      default: {
+        throw new Error("Invalid settings");
+      }
+    }
+    this.effectiveMass *= this.options.strength;
+  }
+  warmStart(direction: Vec3, warmStartImpulseRatio: number): void {
+    this.totalLambda *= warmStartImpulseRatio;
+    this.applyDirectImpulse(direction, this.totalLambda);
+  }
+  solveVelocity(direction: Vec3, minLambda: number, maxLambda: number): void {
+    let totalLambda = this.getTotalLambda(direction);
+    totalLambda *= this.options.velocityBaumgarte;
+    totalLambda = clamp(totalLambda, minLambda, maxLambda);
+    const deltaLambda = totalLambda - this.totalLambda;
+    this.totalLambda = totalLambda;
+    this.applyDirectImpulse(direction, deltaLambda);
+  }
+  solvePosition(direction: Vec3, error: number): void {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    // // TODO: do we need the check of active?
+    // if (error === 0 || this.isActive()) {
+    //   return;
+    // }
+    if (error === 0) {
+      return;
+    }
+    const lambda = this.effectiveMass * error * -this.options.positionBaumgarte;
+    if (bodyA.type === BodyType.dynamic) {
+      linearImpulse.scaleVector(direction, lambda * bodyA.inverseMass);
+      bodyA.computedCenterOfMassPosition.subtractVectors(bodyA.computedCenterOfMassPosition, linearImpulse);
+      angularImpulse.scaleVector(this.mInvI1_R1PlusUxAxis, lambda);
+      bodyA.subRotationDelta(angularImpulse);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      linearImpulse.scaleVector(direction, lambda * bodyB.inverseMass);
+      bodyB.computedCenterOfMassPosition.addVectors(bodyB.computedCenterOfMassPosition, linearImpulse);
+      angularImpulse.scaleVector(this.mInvI2_R2xAxis, lambda);
+      bodyB.addRotationDelta(angularImpulse);
+    }
+  }
+}

package/src/constraints/components/HingeComponent.ts ADDED Viewed

@@ -0,0 +1,215 @@
+import {
+  createClass,
+  LazyReferenceType,
+  MonomorphType,
+  NumberType,
+  PropertyDefinitionMap,
+  PropertyDefinitionReference,
+} from "monomorph";
+import { Body, BodyType } from "../../physics/Body";
+import { ConstraintOptions } from "../ConstraintOptions";
+import { Vec3 } from "../../math/vec3";
+import { Mat3 } from "../../math/mat3";
+import type { World } from "../../world";
+const spinBA = /*@__PURE__*/ Vec3.create();
+const impulse = /*@__PURE__*/ Vec3.create();
+const axisA2 = /*@__PURE__*/ Vec3.create();
+const perpendicular = /*@__PURE__*/ Vec3.create();
+const summedInverseInertia = /*@__PURE__*/ Mat3.create();
+const massBA = /*@__PURE__*/ Vec3.create();
+const massCA = /*@__PURE__*/ Vec3.create();
+const hingeComponentProps = {
+  bodyA: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  bodyB: LazyReferenceType((() => Body) as () => never) as PropertyDefinitionReference<Body | null, true>,
+  options: MonomorphType(ConstraintOptions, {
+    positionBaumgarte: 0.8,
+    velocityBaumgarte: 1.0,
+    strength: 1.0,
+  }),
+  axisA: MonomorphType(Vec3, undefined, true),
+  axisB: MonomorphType(Vec3, undefined, true),
+  axisC: MonomorphType(Vec3, undefined, true),
+  crossBA: MonomorphType(Vec3, undefined, true),
+  crossCA: MonomorphType(Vec3, undefined, true),
+  effectiveMass00: NumberType(0, true),
+  effectiveMass01: NumberType(0, true),
+  effectiveMass10: NumberType(0, true),
+  effectiveMass11: NumberType(0, true),
+  inverseInertiaA: MonomorphType(Mat3, undefined, true),
+  inverseInertiaB: MonomorphType(Mat3, undefined, true),
+  totalLambdaA: NumberType(0, true),
+  totalLambdaB: NumberType(0, true),
+} as const satisfies PropertyDefinitionMap;
+function getOrthogonalVector(out: Vec3, a: Vec3) {
+  // y closer to zero, project onto xz plane
+  if (Math.abs(a.x) > Math.abs(a.y)) {
+    out.fromArray([a.z, 0, -a.x]);
+  }
+  // x closer to zero, project onto yz plane
+  else {
+    out.fromArray([0, a.z, -a.y]);
+  }
+}
+export class HingeComponent extends createClass<HingeComponent, typeof hingeComponentProps>(hingeComponentProps) {
+  declare world: World;
+  deactivate(): void {
+    this.effectiveMass00 = 0;
+    this.effectiveMass01 = 0;
+    this.totalLambdaA = 0;
+    this.totalLambdaB = 0;
+  }
+  applyDirectImpulse(lambdaA: number, lambdaB: number): void {
+    if (lambdaA === 0 && lambdaB === 0) {
+      return;
+    }
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    impulse.zero();
+    impulse.addScaledVector(this.crossBA, lambdaA);
+    impulse.addScaledVector(this.crossCA, lambdaB);
+    if (bodyA.type === BodyType.dynamic) {
+      bodyA.angularVelocity.subRotatedVector(impulse, this.inverseInertiaA);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      bodyB.angularVelocity.addRotatedVector(impulse, this.inverseInertiaB);
+    }
+  }
+  warmStart(warmStartImpulseRatio: number): void {
+    this.totalLambdaA *= warmStartImpulseRatio;
+    this.totalLambdaB *= warmStartImpulseRatio;
+    this.applyDirectImpulse(this.totalLambdaA, this.totalLambdaB);
+  }
+  solveVelocity(): void {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    spinBA.subtractVectors(bodyA.angularVelocity, bodyB.angularVelocity);
+    const jvA = this.crossBA.dot(spinBA);
+    const jvB = this.crossCA.dot(spinBA);
+    const lambdaA = this.options.velocityBaumgarte * (this.effectiveMass00 * jvA + this.effectiveMass10 * jvB);
+    const lambdaB = this.options.velocityBaumgarte * (this.effectiveMass01 * jvA + this.effectiveMass11 * jvB);
+    this.totalLambdaA += lambdaA;
+    this.totalLambdaB += lambdaB;
+    this.applyDirectImpulse(lambdaA, lambdaB);
+  }
+  solvePosition(): void {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    // Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2)
+    const cA = this.axisA.dot(this.axisB);
+    const cB = this.axisA.dot(this.axisC);
+    // TODO: we may need a check for "close to zero" instead of "equal to zero"
+    if (cA == 0 && cB == 0) {
+      return;
+    }
+    const lambdaA = -this.options.positionBaumgarte * (this.effectiveMass00 * cA + this.effectiveMass10 * cB);
+    const lambdaB = -this.options.positionBaumgarte * (this.effectiveMass01 * cA + this.effectiveMass11 * cB);
+    impulse.zero();
+    impulse.addScaledVector(this.crossBA, lambdaA);
+    impulse.addScaledVector(this.crossCA, lambdaB);
+    if (bodyA.type === BodyType.dynamic) {
+      bodyA.subRotationPositionImpulse(impulse, this.inverseInertiaA);
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      bodyB.addRotationPositionImpulse(impulse, this.inverseInertiaB);
+    }
+  }
+  setup(hingeAxisA: Vec3, hingeAxisB: Vec3): void {
+    const bodyA = this.bodyA!;
+    const bodyB = this.bodyB!;
+    this.axisA.copy(hingeAxisA);
+    axisA2.copy(hingeAxisB);
+    let dot = this.axisA.dot(axisA2);
+    if (dot <= 1e-3) {
+      // World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees
+      // perpendicular.copy(axisA2).subScaledVector(this.axisA, dot);
+      perpendicular.addScaledToVector(axisA2, this.axisA, -dot);
+      if (perpendicular.squaredLength() < 1e-6) {
+        // mA1 ~ -a2, take random perpendicular
+        // perpendicular.copy(this.axisA.computeNormalizedPerpendicular());
+        getOrthogonalVector(perpendicular, this.axisA);
+        perpendicular.normalize();
+      }
+      axisA2.zero();
+      axisA2.addScaledVector(perpendicular, 0.99);
+      axisA2.addScaledVector(this.axisA, 0.01);
+      axisA2.normalize();
+    }
+    getOrthogonalVector(this.axisB, axisA2);
+    this.axisB.normalize();
+    this.axisC.crossVectors(axisA2, this.axisB);
+    this.axisC.normalize();
+    // Calculate properties used during constraint solving
+    if (bodyA.type === BodyType.dynamic) {
+      bodyA.computeInverseInertiaTensor(this.inverseInertiaA);
+    } else {
+      this.inverseInertiaA.zero();
+    }
+    if (bodyB.type === BodyType.dynamic) {
+      bodyB.computeInverseInertiaTensor(this.inverseInertiaB);
+    } else {
+      this.inverseInertiaB.zero();
+    }
+    this.crossBA.crossVectors(this.axisB, this.axisA);
+    this.crossCA.crossVectors(this.axisC, this.axisA);
+    summedInverseInertia.addMatrices(this.inverseInertiaA, this.inverseInertiaB);
+    massBA.transformVectorFromMat3(this.crossBA, summedInverseInertia);
+    massCA.transformVectorFromMat3(this.crossCA, summedInverseInertia);
+    const effectiveMass00 = this.crossBA.dot(massBA);
+    const effectiveMass01 = this.crossBA.dot(massCA);
+    const effectiveMass10 = this.crossCA.dot(massBA);
+    const effectiveMass11 = this.crossCA.dot(massCA);
+    const determinant = effectiveMass00 * effectiveMass11 - effectiveMass01 * effectiveMass10;
+    // TODO: we may need a check for "close to zero" instead of "equal to zero"
+    if (determinant === 0) {
+      this.deactivate();
+    } else {
+      this.effectiveMass00 = effectiveMass11 / determinant;
+      this.effectiveMass01 = -effectiveMass10 / determinant;
+      this.effectiveMass10 = -effectiveMass01 / determinant;
+      this.effectiveMass11 = effectiveMass00 / determinant;
+    }
+    this.effectiveMass00 *= this.options.strength;
+    this.effectiveMass01 *= this.options.strength;
+    this.effectiveMass10 *= this.options.strength;
+    this.effectiveMass11 *= this.options.strength;
+  }
+}