From 7eff04742ce0e67fd4b8d205880dbaad711978ea Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 13 Dec 2016 10:17:13 -0500 Subject: [PATCH] Fix for zero check on quat from dcm ctor. --- matrix/Quaternion.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 98f1efddcc..880e63142d 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -98,9 +98,10 @@ public: q(1) = 0; q(2) = 0; q(3) = 0; - if (((Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)) > 0) & (fabsf(q(0)) > 0) ) { - q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); + Type q0_tmp = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) + + dcm(1, 1) + dcm(2, 2))); + if (fabsf(q0_tmp) > 0) { + q(0) = q0_tmp; q(1) = Type((dcm(2, 1) - dcm(1, 2)) / (Type(4) * q(0))); q(2) = Type((dcm(0, 2) - dcm(2, 0)) /