From e0eecd56b2a329775d9fc731ce047a9ce1228558 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Fri, 24 Jun 2016 16:14:52 +0900
Subject: [PATCH] AP_Math: use is_zero and float constants

---
 libraries/AP_Math/quaternion.cpp | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index f299f03a82..a5047b3671 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -99,19 +99,19 @@ void Quaternion::from_rotation_matrix(const Matrix3f &m)
         qy = (m02 - m20) / S;
         qz = (m10 - m01) / S;
     } else if ((m00 > m11) && (m00 > m22)) {
-        float S = sqrtf(1.0f + m00 - m11 - m22) * 2;
+        float S = sqrtf(1.0f + m00 - m11 - m22) * 2.0f;
         qw = (m21 - m12) / S;
         qx = 0.25f * S;
         qy = (m01 + m10) / S;
         qz = (m02 + m20) / S;
     } else if (m11 > m22) {
-        float S = sqrtf(1.0f + m11 - m00 - m22) * 2;
+        float S = sqrtf(1.0f + m11 - m00 - m22) * 2.0f;
         qw = (m02 - m20) / S;
         qx = (m01 + m10) / S;
         qy = 0.25f * S;
         qz = (m12 + m21) / S;
     } else {
-        float S = sqrtf(1.0f + m22 - m00 - m11) * 2;
+        float S = sqrtf(1.0f + m22 - m00 - m11) * 2.0f;
         qw = (m10 - m01) / S;
         qx = (m02 + m20) / S;
         qy = (m12 + m21) / S;
@@ -155,7 +155,7 @@ void Quaternion::from_vector312(float roll ,float pitch, float yaw)
 void Quaternion::from_axis_angle(Vector3f v)
 {
     float theta = v.length();
-    if (theta < 1.0e-12f) {
+    if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
         return;
@@ -166,7 +166,8 @@ void Quaternion::from_axis_angle(Vector3f v)
 
 void Quaternion::from_axis_angle(const Vector3f &axis, float theta)
 {
-    if (theta < 1.0e-12f) {
+    // axis must be a unit vector as there is no check for length
+    if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
     }
@@ -189,7 +190,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
 {
     float l = sqrt(sq(q2)+sq(q3)+sq(q4));
     v = Vector3f(q2,q3,q4);
-    if (l >= 1.0e-12f) {
+    if (!is_zero(l)) {
         v /= l;
         v *= wrap_PI(2.0f * atan2f(l,q1));
     }
@@ -198,7 +199,7 @@ void Quaternion::to_axis_angle(Vector3f &v)
 void Quaternion::from_axis_angle_fast(Vector3f v)
 {
     float theta = v.length();
-    if (theta < 1.0e-12f) {
+    if (is_zero(theta)) {
         q1 = 1.0f;
         q2=q3=q4=0.0f;
     }
@@ -221,7 +222,7 @@ void Quaternion::from_axis_angle_fast(const Vector3f &axis, float theta)
 void Quaternion::rotate_fast(const Vector3f &v)
 {
     float theta = v.length();
-    if (theta < 1.0e-12f) {
+    if (is_zero(theta)) {
         return;
     }
     float t2 = theta/2.0f;
@@ -251,7 +252,7 @@ void Quaternion::rotate_fast(const Vector3f &v)
 // get euler roll angle
 float Quaternion::get_euler_roll() const
 {
-    return (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 2.0f*(q2*q2 + q3*q3)));
+    return (atan2f(2.0f*(q1*q2 + q3*q4), 1.0f - 2.0f*(q2*q2 + q3*q3)));
 }
 
 // get euler pitch angle
@@ -263,7 +264,7 @@ float Quaternion::get_euler_pitch() const
 // get euler yaw angle
 float Quaternion::get_euler_yaw() const
 {
-    return atan2f(2.0f*(q1*q4 + q2*q3), 1 - 2.0f*(q3*q3 + q4*q4));
+    return atan2f(2.0f*(q1*q4 + q2*q3), 1.0f - 2.0f*(q3*q3 + q4*q4));
 }
 
 // create eulers from a quaternion
@@ -295,7 +296,7 @@ Quaternion Quaternion::inverse(void) const
 void Quaternion::normalize(void)
 {
     float quatMag = length();
-    if (quatMag > 1e-16f) {
+    if (!is_zero(quatMag)) {
         float quatMagInv = 1.0f/quatMag;
         q1 *= quatMagInv;
         q2 *= quatMagInv;