Replace linear smoothing with tuned 1€ (OneEuro) filter for position + orientation

public/js/ar/fuse.js

@@ -23,18 +23,26 @@ export class WorldFuser {
     this.anchorMats = new Map(); // markerId -> Matrix4
     this.smoothPos = null;
     this.smoothQuat = null;
-    // adaptive smoothing bounds: minAlpha when nearly still (heavy smoothing,
-    // kills jitter), maxAlpha when moving fast (light smoothing, stays responsive)
-    this.minAlpha = 0.08;
-    this.maxAlpha = 0.6;
+    /* 1€ (OneEuro) filter — the standard cure for marker-pose jitter. It low-passes
+     * hard when the signal is slow (phone still => kills jitter) and eases off as
+     * speed rises (phone moving => stays responsive, no lag). Two knobs:
+     *   minCutoff — lower = steadier at rest (more smoothing when still)
+     *   beta      — higher = snappier when moving (less lag during motion)
+     * dCutoff is the cutoff for the internal speed estimate; 1.0 is standard. */
+    this.oe = {
+      minCutoffPos: 0.6, betaPos: 0.05,
+      minCutoffAng: 0.7, betaAng: 0.06,
+      dCutoff: 1.0,
+      prevPos: null, dPos: new THREE.Vector3(),
+      prevQuat: null, dAngRate: 0,
+    };
     this.lastFuseT = 0;
   }
 
-  // Map a motion magnitude between [lo, hi] thresholds to an alpha in
-  // [minAlpha, maxAlpha]. Below lo => minAlpha (still); above hi => maxAlpha (moving).
-  adaptAlpha(delta, lo, hi) {
-    const tt = Math.min(1, Math.max(0, (delta - lo) / (hi - lo)));
-    return this.minAlpha + (this.maxAlpha - this.minAlpha) * tt;
+  // low-pass alpha from a cutoff frequency (Hz) and timestep dt (s)
+  static alpha(cutoff, dt) {
+    const tau = 1 / (2 * Math.PI * cutoff);
+    return 1 / (1 + tau / dt);
   }
 
   setScene(scene) {
@@ -81,25 +89,40 @@ export class WorldFuser {
     }
     pos.multiplyScalar(1 / wSum);
 
-    // temporal smoothing — adaptive: smooth hard when nearly still (kills jitter),
-    // loosen when genuinely moving (stays responsive). Reset only after a real
-    // tracking gap so brief single-frame dropouts don't cause a visible snap.
+    // ---- 1€ filter (position + orientation) ----
     const now = performance.now();
-    if (this.smoothPos && now - this.lastFuseT < 1500) {
-      // positional delta in cm; rotational delta in radians
-      const dPos = this.smoothPos.distanceTo(pos);
-      if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
-      const dAng = 2 * Math.acos(Math.min(1, Math.abs(this.smoothQuat.dot(quat))));
-      // map motion -> alpha in [minAlpha, maxAlpha]. Small motion => small alpha.
-      const pAlpha = this.adaptAlpha(dPos, 0.5, 6);    // still<0.5cm .. moving>6cm
-      const qAlpha = this.adaptAlpha(dAng, 0.01, 0.15); // still<0.6° .. moving>8.6°
-      this.smoothPos.lerp(pos, pAlpha);
-      this.smoothQuat.slerp(quat, qAlpha);
-    } else {
-      this.smoothPos = pos.clone();
-      this.smoothQuat = quat.clone();
-    }
+    const gap = now - this.lastFuseT;
+    const dt = this.smoothPos ? Math.min(0.1, Math.max(0.001, gap / 1000)) : 1 / 30;
     this.lastFuseT = now;
+    const oe = this.oe;
+
+    // reset on first frame or after a real tracking gap (>1.5s)
+    if (!this.smoothPos || gap > 1500) {
+      this.smoothPos = pos.clone(); oe.prevPos = pos.clone(); oe.dPos.set(0, 0, 0);
+      this.smoothQuat = quat.clone(); oe.prevQuat = quat.clone(); oe.dAngRate = 0;
+      return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
+    }
+
+    // POSITION: derivative -> speed -> dynamic cutoff -> low-pass
+    const dPosRaw = pos.clone().sub(oe.prevPos).multiplyScalar(1 / dt); // cm/s
+    const aD = WorldFuser.alpha(oe.dCutoff, dt);
+    oe.dPos.lerp(dPosRaw, aD);
+    const speed = oe.dPos.length();
+    const cutoffP = oe.minCutoffPos + oe.betaPos * speed;
+    const aP = WorldFuser.alpha(cutoffP, dt);
+    this.smoothPos.lerp(pos, aP);
+    oe.prevPos.copy(pos);
+
+    // ORIENTATION: angular speed -> dynamic cutoff -> slerp
+    if (this.smoothQuat.dot(quat) < 0) quat.set(-quat.x, -quat.y, -quat.z, -quat.w);
+    if (oe.prevQuat.dot(quat) < 0) oe.prevQuat.set(-oe.prevQuat.x, -oe.prevQuat.y, -oe.prevQuat.z, -oe.prevQuat.w);
+    const angDelta = 2 * Math.acos(Math.min(1, Math.abs(oe.prevQuat.dot(quat)))) / dt; // rad/s
+    oe.dAngRate += aD * (angDelta - oe.dAngRate);
+    const cutoffA = oe.minCutoffAng + oe.betaAng * oe.dAngRate;
+    const aA = WorldFuser.alpha(cutoffA, dt);
+    this.smoothQuat.slerp(quat, aA);
+    oe.prevQuat.copy(quat);
+
     return { position: this.smoothPos.clone(), quaternion: this.smoothQuat.clone(), markerCount: cams.length };
   }
 }