boardViewer

public/sceneViewer.html

@@ -483,39 +483,23 @@ function transformDirByT(T, dir) {
   ];
 }
 
-function makeMarkerSquare(pos, normal, size, color) {
+// Marker-Quadrat mit vorab berechneter Orientierung (Quaternion). Die
+// BoxGeometry ist dünn in lokal-Z, ihre Normale ist also lokal +Z — quat
+// dreht +Z auf die Marker-Normale inkl. Link-Rotation und Spin.
+//
+// Die Orientierung MUSS im lokalen Link-Frame gebaut und erst danach in die
+// Szene gedreht werden (siehe Aufrufer). Würde man wie früher
+// setFromUnitVectors([0,0,1], welt_normale) NACH dem robot→three.js-
+// Achsentausch anwenden, verdreht eine schräg liegende Normale (z.B.
+// [-1,0,1]) das Quadrat zusätzlich um ihren Azimut (~45°) um die eigene
+// Achse, und der Spin fehlt ganz. Gegen triangulierte Ecken geprüft
+// (Capture 20260616_133151, Marker 146): lokale Variante 0.8°, alte 45.5°.
+function makeMarkerSquareQuat(pos, quat, size, color) {
   const geo = new THREE.BoxGeometry(size, size, size * 0.1);
-  const mat = new THREE.MeshPhongMaterial({
-    color,
-    shininess: 40
-  });
-
+  const mat = new THREE.MeshPhongMaterial({ color, shininess: 40 });
   const m = new THREE.Mesh(geo, mat);
   m.position.copy(pos);
-
-  // Fallback falls keine gültige Normale vorhanden
-  let nx = 0, ny = 0, nz = 1;
-
-  if (Array.isArray(normal) && normal.length >= 3) {
-    nx = Number(normal[0]) || 0;
-    ny = Number(normal[1]) || 0;
-    nz = Number(normal[2]) || 1;
-  } else if (normal instanceof THREE.Vector3) {
-    nx = normal.x;
-    ny = normal.y;
-    nz = normal.z;
-  }
-
-  const n = new THREE.Vector3(nx, ny, nz);
-
-  if (n.lengthSq() > 1e-12) {
-    n.normalize();
-    m.quaternion.setFromUnitVectors(
-      new THREE.Vector3(0, 0, 1),
-      n
-    );
-  }
-
+  m.quaternion.copy(quat);
   return m;
 }
 
@@ -837,8 +821,19 @@ function rebuild() {
   // ── model markers + normals ──
   const modelPositions = {};
   const modelNormals   = {};
+  // robot→three.js view rotation (x,y,z)->(x,z,-y) == Rot_x(-90°). Applied LAST,
+  // so the marker orientation can be built in the robot/link frame first.
+  const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2);
   for (const [lname, ld] of Object.entries(links)) {
     const col = linkColor(lname);
+    // link rotation in the robot frame (from FK), as a quaternion
+    const Tl = T[lname] || I4();
+    const qLink = new THREE.Quaternion().setFromRotationMatrix(new THREE.Matrix4().set(
+      Tl[0], Tl[1], Tl[2],  0,
+      Tl[4], Tl[5], Tl[6],  0,
+      Tl[8], Tl[9], Tl[10], 0,
+      0,     0,     0,      1
+    ));
     for (const m of (ld.markers||[])) {
       if (!m.position) continue;
       const mid    = m.id;
@@ -849,8 +844,16 @@ function rebuild() {
       modelPositions[mid] = wp;
       modelNormals[mid]   = nWorld;
 
-      const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col);
-      gModel.add(sq);
+      // Orientierung ZUERST im lokalen Link-Frame (robot): Minimal-Rotation
+      // [0,0,1]→Normale, dann Spin um diese Normale; DANN qLink (Link-Drehung)
+      // und qView (in die Szene). So bleibt der Roll des Links um die Normale
+      // erhalten und der Spin-Azimut-Twist entfällt (siehe makeMarkerSquareQuat).
+      const nLR     = new THREE.Vector3(nLocal[0], nLocal[1], nLocal[2]).normalize();
+      const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
+      const qNormal = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLR);
+      const qSpin   = new THREE.Quaternion().setFromAxisAngle(nLR, spinRad);
+      const qMarker = qView.clone().multiply(qLink).multiply(qSpin.multiply(qNormal));
+      gModel.add(makeMarkerSquareQuat(r2vArr(wp), qMarker, 0.022, col));
 
       // normal arrow  (length = half a marker size = ~12.5mm → 0.0125m)
       const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col);