tests

package.json

@@ -23,8 +23,8 @@
   },
   "devDependencies": {
     "esbuild": "^0.28.0",
-    "jest": "^30.3.0",
+    "jest": "^29.7.0",
-    "jest-environment-jsdom": "^30.3.0",
+    "jest-environment-jsdom": "^29.7.0",
     "nodemon": "^3.0.2"
   }
 }

public/calculateAngles.js

@@ -5,6 +5,43 @@
  */
 
 
+
+function v_multiplication(vec, scalar){
+  if(vec.length == 3){
+    return [vec[0]*scalar, vec[1]*scalar, vec[2]*scalar]
+  }
+}
+
+function v_dot(v1, v2) {
+  return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
+}
+
+function v_length(v) {
+  return Math.sqrt(v_dot(v, v));
+}
+
+function v_minus(v1, v2){
+  if(v1.length == 3 && v2.length==3)
+    return [v1[0] - v2[0], v1[1] - v2[1], v1[2] - v2[2]]
+  else
+    throw "Vektor Length is wrong";
+}
+
+function v_angle(v1, v2) {
+  const cosTheta = v_dot(v1, v2) / (v_length(v1) * v_length(v2));
+  
+  // kleine numerische Fehler abfangen
+  const clamped = Math.max(-1, Math.min(1, cosTheta));
+  
+  return Math.acos(clamped); // Winkel in Radiant
+}
+
+function v_getProjectionToPlane(vec, normal){
+    return v_minus(vec,  v_multiplication(normal, v_dot(vec, normal)) )
+}
+
+
+
 function getAnalysisLogEl() {
   if (typeof document === "undefined") return null;
   return document.getElementById("analysis-log");
@@ -123,7 +160,7 @@ function optimizeRobot(listFoundMarkers, jsonRobot) {
   return withStats;
 }
 
-function calculateRotationAngleByTwoMarkers(listIdAndX, jsonRobot, jointName){
+function calculateAngle_byRelativePositionOfMarker(listRecoginize, jsonRobot, jointName){
 
   // Achse finden
   const jointInfo = jsonRobot.Joints[jointName];
@@ -133,52 +170,70 @@ function calculateRotationAngleByTwoMarkers(listIdAndX, jsonRobot, jointName){
   if(!(jointInfo.axis)){ return null, null; }
   if(!(jointInfo.child)){ return null, null; }
 
-  var a = 1;
-  var b = 2;
-  if(jointInfo.axis == [1,0,0]){a=1, b=2}
-  if(jointInfo.axis == [0,1,0]){a=0, b=2}
-  if(jointInfo.axis == [0,0,1]){a=0, b=1}
-
   const achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a" 
   
-
+  appendToAnalysis(`${achsisName}Ang - RelativePosition Started with  n=[${jointInfo.axis}]`)
-  appendToAnalysis(`${achsisName}Ang - Double Started with on dir [${a}, ${b}]`)
 
   markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
-  if(markerUsed.length === 0){ 
+  if(markerUsed.length < 2 ){ 
-    appendToAnalysis(`${achsisName}Ang - Double no Markers found`)
+    appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs on Robot`)
     return null, null; 
   }
 
   const markerFound = markerUsed
-  .map(m => [m.id, listIdAndX.get(m.id)])
+  .map(m => [m.id, listRecoginize.get(m.id)])
   .filter(v => v !== undefined && v[1] !== undefined);
 
-   
+  if(markerFound.length < 2 ){ 
-  const pairs = [];
+    appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs found in Fotos`)
-  for (let i = 0; i < markerFound.length; i++) {
+    return null, null; 
-    for (let j = i + 1; j < markerFound.length; j++) {
-      const [idA, posA] = markerFound[i];
-      const [idB, posB] = markerFound[j];
-
-      if (posA[a] === posB[a] && posA[b] === posB[b]) {
-        pairs.push([idA, idB]);
-      }
-    }
   }
 
+ 
+  const pairs = markerFound.flatMap((a, i) =>
+    markerFound.slice(i + 1).map(b => [a[0], b[0]])
+  );
+
+
   if(pairs == []){ appendToAnalysis(`${achsisName}Ang - Double not found`); return; }
 
+  const n = v_multiplication(jointInfo.axis, 1/(Math.sqrt(v_dot(jointInfo.axis,jointInfo.axis))))
 
-  for(i in pairs){
+  const markerMap = Object.fromEntries(
-    m0 = listIdAndX.get(i[0]);
+    jsonRobot.Marker.map(m => [m.id, m])
-    m1 = listIdAndX.get(i[1]);
+  );
 
-    appendToAnalysis(`${achsisName}Ang - Double ${JSON.stringify(m0)} and ${JSON.stringify(m1)} -- ${i}`);
+  const pairsWithAngles = pairs.map(([id0, id1]) => {
-  }
+
+    // Point in real World
+    const m0p = v_getProjectionToPlane(listRecoginize.get(id0), n);
+    const m1p = v_getProjectionToPlane(listRecoginize.get(id1), n);
+
+    // Point in Robot Model
+    const f0p = v_getProjectionToPlane(markerMap[id0].relPos, n);
+    const f1p = v_getProjectionToPlane(markerMap[id1].relPos, n);
+
+    const angleMarker = v_angle(m0p, m1p) * 180 / Math.PI;
+    const angleFound  = v_angle(f0p, f1p) * 180 / Math.PI;
+
+    const lengthProjM = v_length(v_minus(m0p, m1p));
+    const lengthProjF = v_length(v_minus(f0p, f1p));
+
+    return {
+      pair: [id0, id1],
+      angleMarker,
+      angleFound,
+      rotationAngle: angleMarker - angleFound,
+      lengthProjM,
+      lengthProjF
+    };
+  });
+
+
+  appendToAnalysis(`${achsisName}Ang - Pairs with Angles: ${JSON.stringify(pairsWithAngles)}`);
 }
 
-function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
+function calculateAngle_byPosAndAxis(listIdAndX, jsonRobot, jointName, method = "tan") {
 
   // Achse finden
   const jointInfo = jsonRobot.Joints[jointName];
@@ -289,21 +344,17 @@ async function calculate(foundMarkers, jsonRobot) {
 
   jsonRobot.recognized.x = x;
 
-  const { average: y, deviation: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
+  const { average: y, deviation: vary } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointB", "tan");
+  
+  calculateAngle_byRelativePositionOfMarker(foundById, jsonRobot, "jointB");
   
-  try{
-    calculateRotationAngleByTwoMarkers(foundById, jsonRobot, "jointB");
-  }
-  catch(e){
-    appendToAnalysis(e);
-  }
   jsonRobot.recognized.y = y;
   // ToDo !  callibration
   jsonRobot.Joints["jointD"].origin[0] = x;
   jsonRobot.Joints["jointD"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)
   jsonRobot.Joints["jointD"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)
   
-  const { average: a, deviation: vara } = calculateRotationAngle(foundById, jsonRobot, "jointD", "sin");
+  const { average: a, deviation: vara } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointD", "sin");
 
   jsonRobot.Joints["jointC"].origin[0] = x;
   jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)

public/robot.json

@@ -23,10 +23,10 @@
         {"id":201,"on":"Base","relPos":[-164.8,  97.5,  74.5], "relPosSource":["226_FotoAverage_5_2026",null,null]},
         {"id":204,"on":"Base","relPos":[-158.5,152.5,111]},
               
-        {"id":198,"on":"Arm1", "relPos":[-93.4,-160, 35], "relPosSource":["226_FotoAverage_5_2026",null,null]},  
+        {"id":198,"on":"Arm1","relPos":[-89.5,-160, 35],"relPosSource":["Fusion",null,null]},  
-        {"id":229,"on":"Arm1", "relPos":[-90.6,-250, 35], "relPosSource":["226_FotoAverage_5_2026",null,null]},        
+        {"id":229,"on":"Arm1","relPos":[-89.5,-250, 35],"relPosSource":["Fusion",null,null]},        
-        {"id":242,"on":"Arm1", "relPos":[-89.5,-250,-35]},        
+        {"id":242,"on":"Arm1","relPos":[-89.5,-250,-35]},        
-        {"id":243,"on":"Arm1", "relPos":[-89.5,-285,  0]}, 
+        {"id":243,"on":"Arm1","relPos":[-89.5,-285,  0]}, 
 
         {"id":222,"on":"Joint1", "relPos":[0,0, -35]},
         {"id":226,"on":"Joint1", "relPos":[0,0, 35]},

test/calculateAngles3_Calculate_DoubleMarker.test.js

@@ -0,0 +1,34 @@
+// __tests__/calculateAngles.test.js
+
+const fs = require('fs');
+const path = require('path');
+
+const { calculate } = require('../public/calculateAngles');
+
+describe('calculateAngles minimal test', () => {
+  it('should run calculate() with loaded JSON files', async () => {
+    // Pfade auflösen
+    const robotPath = path.resolve(__dirname, '../public/robot.json');
+    const snapshotPath = path.resolve(
+      __dirname,
+      '../test/snapshots/snapshot_video0_1778845508432_two_cam.json'
+    );
+
+    // JSON laden
+    var f = fs.readFileSync(robotPath, 'utf-8').replace(/^\uFEFF/, "").trim();
+    var jsonRobot = null;
+    try{
+     jsonRobot = JSON.parse(f);
+    }
+    catch(e){
+      console.log(e)
+    }
+     const foundMarkers = JSON.parse(fs.readFileSync(snapshotPath, 'utf-8'));
+
+    // Funktion aufrufen
+    const result = await calculate(foundMarkers, jsonRobot);
+
+    // Minimal Assertion: Ergebnis existiert
+    expect(result).toBeDefined();
+  });
+});