200 lines
8.0 KiB
JavaScript
200 lines
8.0 KiB
JavaScript
/**
|
||
* yAxisCompute.js
|
||
* ================
|
||
* Reine Mathematik zur Rotationsachsen-Berechnung aus drei Marker-Positionen.
|
||
* Keine DOM- oder Three.js-Abhängigkeit – läuft in Browser und Node.js.
|
||
*
|
||
* UMD-Pattern:
|
||
* Browser: window.YAxisCompute.computeYAxis(...)
|
||
* Node.js: const { computeYAxis } = require('./public/yAxisCompute')
|
||
*
|
||
* Eingabe (markersA/B/C):
|
||
* Array von { marker_id: number, position_mm: [x, y, z] }
|
||
* – entspricht den fremd-Markern (link !== 'Board') aus aruco_marker_poses.json
|
||
*
|
||
* Ausgabe bei Erfolg:
|
||
* {
|
||
* ok: true,
|
||
* axisDir: [x, y, z], // Einheitsvektor der Rotationsachse
|
||
* axisPoint: [x, y, z], // Referenzpunkt auf der Achse (mm)
|
||
* tiltXY: degrees, // Abweichung von Y in XY-Ebene
|
||
* tiltYZ: degrees, // Abweichung von Y in YZ-Ebene
|
||
* numMarkers: number, // Anzahl genutzte Marker
|
||
* numMarkersCommon: number, // Alle gemeinsamen Marker (inkl. gefiltert)
|
||
* skipped: [{id, reason, maxMoveMm, posA?}],
|
||
* markerData: [{markerId, posA, posB, posC, circumcenter, normal}],
|
||
* }
|
||
*
|
||
* Ausgabe bei Fehler:
|
||
* { ok: false, reason: string, skipped: [] }
|
||
*/
|
||
(function (exports) {
|
||
'use strict';
|
||
|
||
/** Marker mit max. Zentren-Bewegung unter diesem Wert werden ignoriert. */
|
||
const DEFAULT_MIN_MOVEMENT_MM = 10.0;
|
||
|
||
// ── Hilfsfunktionen ──────────────────────────────────────────────────────────
|
||
|
||
function dist2(P, Q) {
|
||
return (P[0] - Q[0]) ** 2 + (P[1] - Q[1]) ** 2 + (P[2] - Q[2]) ** 2;
|
||
}
|
||
|
||
/**
|
||
* Berechnet:
|
||
* - Umkreismittelpunkt C des Dreiecks P1-P2-P3 (Punkt auf der Rotationsachse)
|
||
* - Normalenvektor n der Dreiecks-Ebene (= Richtung der Rotationsachse)
|
||
*
|
||
* Gibt null zurück wenn das Triplet degenerat ist (kollinear / zu nahe beieinander).
|
||
*/
|
||
function circumcenterAndNormal(P1, P2, P3) {
|
||
const v1 = [P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]];
|
||
const v2 = [P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]];
|
||
const cross = [
|
||
v1[1] * v2[2] - v1[2] * v2[1],
|
||
v1[2] * v2[0] - v1[0] * v2[2],
|
||
v1[0] * v2[1] - v1[1] * v2[0],
|
||
];
|
||
const crossLen = Math.sqrt(cross[0] ** 2 + cross[1] ** 2 + cross[2] ** 2);
|
||
if (crossLen < 1e-3) return null;
|
||
|
||
const n = cross.map(c => c / crossLen);
|
||
|
||
// Baryzentrische Gewichte → Umkreismittelpunkt (doc/Kalibrierung.md → [4] Arm1, Verfahren B)
|
||
const a2 = dist2(P2, P3), b2 = dist2(P1, P3), c2 = dist2(P1, P2);
|
||
const w1 = a2 * (b2 + c2 - a2);
|
||
const w2 = b2 * (a2 + c2 - b2);
|
||
const w3 = c2 * (a2 + b2 - c2);
|
||
const wSum = w1 + w2 + w3;
|
||
if (Math.abs(wSum) < 1e-6) return null;
|
||
|
||
const C = [
|
||
(w1 * P1[0] + w2 * P2[0] + w3 * P3[0]) / wSum,
|
||
(w1 * P1[1] + w2 * P2[1] + w3 * P3[1]) / wSum,
|
||
(w1 * P1[2] + w2 * P2[2] + w3 * P3[2]) / wSum,
|
||
];
|
||
return { C, n };
|
||
}
|
||
|
||
// ── Kern-API ─────────────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Berechnet die Rotationsachse aus drei Sätzen fremd-Marker.
|
||
*
|
||
* @param {Array} markersA Fremd-Marker aus Pos A (link !== 'Board')
|
||
* @param {Array} markersB Fremd-Marker aus Pos B
|
||
* @param {Array} markersC Fremd-Marker aus Pos C
|
||
* @param {Object} options
|
||
* @param {number} [options.minMovementMm=10] Mindest-Bewegung in mm
|
||
*/
|
||
function computeYAxis(markersA, markersB, markersC, options) {
|
||
const minMovementMm = (options && options.minMovementMm != null)
|
||
? options.minMovementMm
|
||
: DEFAULT_MIN_MOVEMENT_MM;
|
||
|
||
const mapA = new Map(markersA.map(m => [m.marker_id, m]));
|
||
const mapB = new Map(markersB.map(m => [m.marker_id, m]));
|
||
const mapC = new Map(markersC.map(m => [m.marker_id, m]));
|
||
|
||
const circumcenters = []; // [{id, C:[x,y,z]}]
|
||
const normals = []; // [[nx, ny, nz]]
|
||
const markerData = []; // [{markerId, posA, posB, posC, circumcenter, normal}]
|
||
const skipped = []; // [{id, reason, maxMoveMm, posA?}]
|
||
|
||
for (const [id, ma] of mapA) {
|
||
const mb = mapB.get(id);
|
||
const mc = mapC.get(id);
|
||
if (!mb || !mc) continue;
|
||
|
||
// Fehlende position_mm (z.B. Einzelkamera-Marker, von 3b nicht
|
||
// trianguliert) ignorieren statt crashen — Marker bleibt im skipped-Log.
|
||
if (!Array.isArray(ma.position_mm) || !Array.isArray(mb.position_mm) || !Array.isArray(mc.position_mm)) {
|
||
skipped.push({ id, reason: 'fehlende position_mm (z.B. Einzelkamera-Marker, nicht trianguliert)' });
|
||
continue;
|
||
}
|
||
|
||
const P1 = ma.position_mm.map(Number);
|
||
const P2 = mb.position_mm.map(Number);
|
||
const P3 = mc.position_mm.map(Number);
|
||
|
||
// ── Mindest-Bewegungs-Filter ──────────────────────────────────────────
|
||
const maxMoveMm = Math.max(
|
||
Math.sqrt(dist2(P1, P2)),
|
||
Math.sqrt(dist2(P2, P3)),
|
||
Math.sqrt(dist2(P1, P3)),
|
||
);
|
||
if (maxMoveMm < minMovementMm) {
|
||
skipped.push({
|
||
id,
|
||
reason: 'Bewegung zu gering (kein rotierender Marker)',
|
||
maxMoveMm: +maxMoveMm.toFixed(2),
|
||
posA: P1, // Positions-Info für späteres Zuordnen zum Base-Link
|
||
});
|
||
continue;
|
||
}
|
||
|
||
// ── Umkreismittelpunkt + Normalenvektor ───────────────────────────────
|
||
const result = circumcenterAndNormal(P1, P2, P3);
|
||
if (!result) {
|
||
skipped.push({ id, reason: 'degenerat (kollinear / identisch)', maxMoveMm: +maxMoveMm.toFixed(2) });
|
||
continue;
|
||
}
|
||
|
||
const { C, n } = result;
|
||
circumcenters.push({ id, C });
|
||
normals.push(n);
|
||
markerData.push({ markerId: id, posA: P1, posB: P2, posC: P3, circumcenter: C, normal: n });
|
||
}
|
||
|
||
if (circumcenters.length === 0) {
|
||
return {
|
||
ok: false,
|
||
reason: skipped.length
|
||
? `Alle ${skipped.length} gemeinsamen Marker gefiltert (Bewegung < ${minMovementMm} mm)`
|
||
: 'Keine gemeinsamen fremd-Marker in Pos A+B+C gefunden',
|
||
skipped,
|
||
};
|
||
}
|
||
|
||
// ── Achsenrichtung: Mittlere Normale (Vorzeichen angleichen) ─────────────
|
||
const ref = normals[0];
|
||
const aligned = normals.map(n => {
|
||
const dot = n[0] * ref[0] + n[1] * ref[1] + n[2] * ref[2];
|
||
return dot >= 0 ? n : n.map(c => -c);
|
||
});
|
||
const meanN = [0, 1, 2].map(i =>
|
||
aligned.reduce((s, n) => s + n[i], 0) / aligned.length
|
||
);
|
||
const meanNLen = Math.sqrt(meanN[0] ** 2 + meanN[1] ** 2 + meanN[2] ** 2);
|
||
const axisDir = meanN.map(c => c / meanNLen);
|
||
|
||
// ── Referenzpunkt: Schwerpunkt der Umkreismittelpunkte ───────────────────
|
||
const axisPoint = [0, 1, 2].map(i =>
|
||
circumcenters.reduce((s, c) => s + c.C[i], 0) / circumcenters.length
|
||
);
|
||
|
||
// ── Kippwinkel gegen Y-Achse [0,1,0] ────────────────────────────────────
|
||
const [ax, ay, az] = axisDir;
|
||
const tiltXY = Math.atan2(ax, ay) * 180 / Math.PI;
|
||
const tiltYZ = Math.atan2(az, ay) * 180 / Math.PI;
|
||
|
||
return {
|
||
ok: true,
|
||
axisDir,
|
||
axisPoint,
|
||
tiltXY,
|
||
tiltYZ,
|
||
numMarkers: circumcenters.length,
|
||
numMarkersCommon: circumcenters.length + skipped.length,
|
||
skipped,
|
||
markerData,
|
||
};
|
||
}
|
||
|
||
// ── Exports ──────────────────────────────────────────────────────────────────
|
||
|
||
exports.computeYAxis = computeYAxis;
|
||
exports.DEFAULT_MIN_MOVEMENT_MM = DEFAULT_MIN_MOVEMENT_MM;
|
||
|
||
}(typeof module !== 'undefined' ? module.exports : (self.YAxisCompute = {})));
|