From 13ae3e74a21407d335fd2c9ec2c9085ee52ccdc0 Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Thu, 20 Feb 2025 20:43:17 +0300 Subject: [PATCH] Added quaternions checking to prevent negative sqrt argument in attitudes calculation algorithm (#813) * Added quaternions checking to prevent negative sqrt argument * Quaternion normalize in case of its module more than 1 Co-authored-by: Petr Ledvina * Quaternions checking code improvement * The math issue is resolved --------- Co-authored-by: Petr Ledvina --- src/flightlog.js | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/flightlog.js b/src/flightlog.js index 9afafb27..7475ee58 100644 --- a/src/flightlog.js +++ b/src/flightlog.js @@ -737,7 +737,19 @@ export function FlightLog(logData) { z: srcFrame[imuQuaternion[2]] / scaleFromFixedInt16, w: 1.0, }; - q.w = Math.sqrt(1.0 - (q.x ** 2 + q.y ** 2 + q.z ** 2)); + + let m = q.x ** 2 + q.y ** 2 + q.z ** 2; + if (m < 1.0) { + // reconstruct .w of unit quaternion + q.w = Math.sqrt(1.0 - m); + } else { + // normalize [0,x,y,z] + m = Math.sqrt(m); + q.x /= m; + q.y /= m; + q.z /= m; + q.w = 0; + } const xx = q.x ** 2, xy = q.x * q.y, xz = q.x * q.z,