From d26628d20b058fb12112ee976053b47d33382aea Mon Sep 17 00:00:00 2001
From: jessikitty <jess.rogerson.29@outlook.com>
Date: Wed, 8 Jul 2026 19:32:04 +1000
Subject: [PATCH] Anchor combining

---
 public/js/exhibit.js | 88 ++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 81 insertions(+), 7 deletions(-)

diff --git a/public/js/exhibit.js b/public/js/exhibit.js
index ffa0b36..a91d552 100644
--- a/public/js/exhibit.js
+++ b/public/js/exhibit.js
@@ -182,6 +182,70 @@ function alignWorld(m2c, anchor) {
   lastWorldToCamera = chosen.clone();
 }
 
+// ---- Multi-marker fusion --------------------------------------------------
+// When several crests are visible at once, each yields its own world->camera
+// estimate. Picking a single "winner" makes the world snap as detection flickers
+// between them. Because every crest's world position is known and the rig is
+// rigid, we instead FUSE all visible estimates into one consensus pose:
+//   * for each marker, resolve its two POS-IT solutions to the one consistent
+//     with last frame (defeats the planar flip), producing one world->camera;
+//   * weight each by confidence (bigger, lower-error, non-coasting = more trust);
+//   * average translations linearly and rotations via quaternion slerp.
+// Result: smoother and more accurate than any single marker, with no snap.
+const _p = new THREE.Vector3(), _q = new THREE.Quaternion(), _s = new THREE.Vector3();
+
+function resolveMarkerWorldToCamera(m2c, anchor) {
+  // returns the single world->camera for this marker, flip-resolved vs last frame
+  const aToW = anchorToWorldMatrix(anchor);
+  const inv = new THREE.Matrix4().copy(aToW).invert();
+  const candBest = worldToCameraFor(m2c.best.M, inv);
+  if (!m2c.alt) return candBest;
+  const candAlt = worldToCameraFor(m2c.alt.M, inv);
+  if (!lastWorldToCamera) {
+    return (m2c.alt.err < m2c.best.err) ? candBest : candBest; // first lock: best
+  }
+  return matDist(candAlt, lastWorldToCamera) < matDist(candBest, lastWorldToCamera)
+    ? candAlt : candBest;
+}
+
+function fuseWorld(observations) {
+  // observations: [{ m2c, anchor, weight }]
+  if (observations.length === 0) return false;
+
+  // Resolve each to a single world->camera, decompose to pos+quat.
+  const parts = [];
+  for (const o of observations) {
+    const wc = resolveMarkerWorldToCamera(o.m2c, o.anchor);
+    const p = new THREE.Vector3(), q = new THREE.Quaternion(), s = new THREE.Vector3();
+    wc.decompose(p, q, s);
+    parts.push({ p, q, w: o.weight });
+  }
+
+  // Weighted translation average.
+  const totalW = parts.reduce((a, b) => a + b.w, 0) || 1;
+  const pos = new THREE.Vector3();
+  for (const pt of parts) pos.addScaledVector(pt.p, pt.w / totalW);
+
+  // Weighted rotation average via incremental slerp. Keep all quats on the same
+  // hemisphere as the first (q and -q are the same rotation; slerp needs care).
+  const q0 = parts[0].q.clone();
+  let quat = q0.clone();
+  let accum = parts[0].w;
+  for (let i = 1; i < parts.length; i++) {
+    let qi = parts[i].q.clone();
+    if (q0.dot(qi) < 0) qi.set(-qi.x, -qi.y, -qi.z, -qi.w); // hemisphere align
+    accum += parts[i].w;
+    quat.slerp(qi, parts[i].w / accum); // incremental weighted mean
+    quat.normalize();
+  }
+
+  const fused = new THREE.Matrix4().compose(pos, quat, new THREE.Vector3(1, 1, 1));
+  world.matrix.copy(fused);
+  world.matrixWorldNeedsUpdate = true;
+  lastWorldToCamera = fused.clone();
+  return true;
+}
+
 // ---- Build occluders + anchor viz once scene arrives ----------------------
 function buildStaticWorld() {
   // occluders: invisible depth-only boxes so ghosts hide behind buildings
@@ -355,21 +419,31 @@ function loop(now) {
   try { raw = detector.detect(img, { width: grab.width, height: grab.height }); } catch (_) { raw = []; }
   const markers = tracker.update(raw, now);
 
-  // 2) pick the best KNOWN marker (one whose id is an anchor) to align the world.
-  //    Prefer the nearest, non-coasting, recognised marker.
-  let best = null;
+  // 2) Fuse ALL visible KNOWN markers into one consensus world pose.
+  //    Each contributes an estimate weighted by confidence (area, low pose
+  //    error, not coasting). Fusing removes the snap you get when a single
+  //    "winner" marker flickers between two visible crests, and is more
+  //    accurate because the rig geometry is known and rigid.
+  const observations = [];
+  let anyLive = false;
   for (const m of markers) {
     if (!anchorById.has(m.id)) continue;
     const m2c = markerToCameraMatrix(m);
     if (!m2c) continue;
-    if (!best || (!m.coasting && m2c.dist < best.dist)) best = { m, m2c, dist: m2c.dist, coasting: m.coasting };
+    // confidence: lower pose error + live (not coasting) + nearer = more trust
+    const errW = 1 / (1 + (m2c.best.err ?? 0));       // lower error -> higher weight
+    const coastW = m.coasting ? 0.35 : 1;             // trust live reads more
+    const nearW = 1 / (1 + m2c.dist / 1000);          // nearer markers slightly favoured
+    const weight = errW * coastW * nearW;
+    observations.push({ m2c, anchor: anchorById.get(m.id), weight });
+    if (!m.coasting) anyLive = true;
   }
 
-  if (best) {
-    alignWorld(best.m2c, anchorById.get(best.m.id));
+  if (observations.length > 0) {
+    fuseWorld(observations);
     aligned = true; lastAlign = now;
     world.visible = true;
-    setLock(best.coasting ? 'holding' : 'locked');
+    setLock(anyLive ? 'locked' : 'holding');
   } else if (aligned && now - lastAlign < REALIGN_GRACE_MS) {
     // keep showing world briefly using last alignment
     setLock('holding');