Y-Axis checks
This commit is contained in:
chk
192
public/yAxisCompute.js
Normal file
192
public/yAxisCompute.js
Normal file
@@ -0,0 +1,192 @@
|
||||
/**
|
||||
* 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/04_y_achse.md)
|
||||
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;
|
||||
|
||||
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 = {})));
|
||||
Reference in New Issue
Block a user