// Local copies of the SDK's pose math, used only by demo mode so it runs // without loading the SDK. Same conventions as the SDK: angles in degrees, // ZYX order (yaw, then pitch, then roll), 4×4 matrix as 16 floats row-major. const D2R = Math.PI / 180; export function rpyToMatrix(roll, pitch, yaw) { const [cr, sr] = [Math.cos(roll * D2R), Math.sin(roll * D2R)]; const [cp, sp] = [Math.cos(pitch * D2R), Math.sin(pitch * D2R)]; const [cy, sy] = [Math.cos(yaw * D2R), Math.sin(yaw * D2R)]; return [ cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, 0, sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, 0, -sp, cp * sr, cp * cr, 0, 0, 0, 0, 1, ]; } export function matrixToRpy(m) { const f = flattenMatrix(m); const pitch = Math.asin(Math.max(-1, Math.min(1, -f[8]))); const roll = Math.atan2(f[9], f[10]); const yaw = Math.atan2(f[4], f[0]); return { roll: roll / D2R, pitch: pitch / D2R, yaw: yaw / D2R }; } /** Accepts a nested 4×4 array or a flat 16-array and returns a flat copy. */ export function flattenMatrix(m) { return Array.isArray(m?.[0]) ? m.flat() : Array.from(m); }