Rationalize QQuaternion's length-scaling code

Use qHypot() to implement length(), avoid duplicating its code and use
its result more carefully, saving the need for casting to and from
double.

Subtracting a double from 1.0f still got a double, so the
qFuzzyIsNull() checks were using double's tolerance, where the use of
1.0f indicates the float tolerance would have been more apt. Also, use
qFuzzyCompare(_, 1.0f) instead of qFuzzyIsNull(_ - 1.0f).

In getEulerAngles(), scale co-ordinates by length before multiplying
(to ensure O(1) quantities) rather than scaling the products by the
squared length (possibly after {ov,und}erflowing).

Change-Id: Id8792d6eb047ee9567a9bbb246657b0217b0849f
Reviewed-by: Andreas Buhr <andreas.buhr@qt.io>
Reviewed-by: Laszlo Agocs <laszlo.agocs@qt.io>
Reviewed-by: Qt CI Bot <qt_ci_bot@qt-project.org>
Reviewed-by: Andrei Golubev <andrei.golubev@qt.io>

src/gui/math3d/qquaternion.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 **
-** Copyright (C) 2016 The Qt Company Ltd.
+** Copyright (C) 2020 The Qt Company Ltd.
 ** Contact: https://www.qt.io/licensing/
 **
 ** This file is part of the QtGui module of the Qt Toolkit.
@@ -234,12 +234,15 @@ QT_BEGIN_NAMESPACE
 */
 float QQuaternion::length() const
 {
-    return std::sqrt(xp * xp + yp * yp + zp * zp + wp * wp);
+    return qHypot(xp, yp, zp, wp);
 }
 
 /*!
     Returns the squared length of the quaternion.
 
+    \note Though cheap to compute, this is susceptible to overflow and underflow
+    that length() avoids in many cases.
+
     \sa length(), dotProduct()
 */
 float QQuaternion::lengthSquared() const
@@ -259,17 +262,12 @@ float QQuaternion::lengthSquared() const
 */
 QQuaternion QQuaternion::normalized() const
 {
-    // Need some extra precision if the length is very small.
+    const float scale = length();
-    double len = double(xp) * double(xp) +
+    if (qFuzzyCompare(scale, 1.0f))
-                 double(yp) * double(yp) +
-                 double(zp) * double(zp) +
-                 double(wp) * double(wp);
-    if (qFuzzyIsNull(len - 1.0f))
         return *this;
-    else if (!qFuzzyIsNull(len))
+    if (qFuzzyIsNull(scale))
-        return *this / std::sqrt(len);
-    else
         return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f);
+    return *this / scale;
 }
 
 /*!
@@ -280,16 +278,10 @@ QQuaternion QQuaternion::normalized() const
 */
 void QQuaternion::normalize()
 {
-    // Need some extra precision if the length is very small.
+    const float len = length();
-    double len = double(xp) * double(xp) +
+    if (qFuzzyCompare(len, 1.0f) || qFuzzyIsNull(len))
-                 double(yp) * double(yp) +
-                 double(zp) * double(zp) +
-                 double(wp) * double(wp);
-    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
         return;
 
-    len = std::sqrt(len);
-
     xp /= len;
     yp /= len;
     zp /= len;
@@ -421,24 +413,22 @@ void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) co
     // The quaternion representing the rotation is
     //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
 
-    float length = xp * xp + yp * yp + zp * zp;
+    const float length = qHypot(xp, yp, zp);
     if (!qFuzzyIsNull(length)) {
-        *x = xp;
+        if (qFuzzyCompare(length, 1.0f)) {
-        *y = yp;
+            *x = xp;
-        *z = zp;
+            *y = yp;
-        if (!qFuzzyIsNull(length - 1.0f)) {
+            *z = zp;
-            length = std::sqrt(length);
+        } else {
-            *x /= length;
+            *x = xp / length;
-            *y /= length;
+            *y = yp / length;
-            *z /= length;
+            *z = zp / length;
         }
-        *angle = 2.0f * std::acos(wp);
+        *angle = qRadiansToDegrees(2.0f * std::acos(wp));
     } else {
         // angle is 0 (mod 2*pi), so any axis will fit
         *x = *y = *z = *angle = 0.0f;
     }
-
-    *angle = qRadiansToDegrees(*angle);
 }
 
 /*!
@@ -450,8 +440,8 @@ void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) co
 QQuaternion QQuaternion::fromAxisAndAngle
         (float x, float y, float z, float angle)
 {
-    float length = std::sqrt(x * x + y * y + z * z);
+    float length = qHypot(x, y, z);
-    if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) {
+    if (!qFuzzyCompare(length, 1.0f) && !qFuzzyIsNull(length)) {
         x /= length;
         y /= length;
         z /= length;
@@ -501,31 +491,25 @@ void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const
 {
     Q_ASSERT(pitch && yaw && roll);
 
-    // Algorithm from:
+    // Algorithm adapted from:
     // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q37
 
-    float xx = xp * xp;
+    const float len = length();
-    float xy = xp * yp;
+    const bool rescale = !qFuzzyCompare(len, 1.0f) && !qFuzzyIsNull(len);
-    float xz = xp * zp;
+    const float xps = rescale ? xp / len : xp;
-    float xw = xp * wp;
+    const float yps = rescale ? yp / len : yp;
-    float yy = yp * yp;
+    const float zps = rescale ? zp / len : zp;
-    float yz = yp * zp;
+    const float wps = rescale ? wp / len : wp;
-    float yw = yp * wp;
-    float zz = zp * zp;
-    float zw = zp * wp;
 
-    const float lengthSquared = xx + yy + zz + wp * wp;
+    const float xx = xps * xps;
-    if (!qFuzzyIsNull(lengthSquared - 1.0f) && !qFuzzyIsNull(lengthSquared)) {
+    const float xy = xps * yps;
-        xx /= lengthSquared;
+    const float xz = xps * zps;
-        xy /= lengthSquared; // same as (xp / length) * (yp / length)
+    const float xw = xps * wps;
-        xz /= lengthSquared;
+    const float yy = yps * yps;
-        xw /= lengthSquared;
+    const float yz = yps * zps;
-        yy /= lengthSquared;
+    const float yw = yps * wps;
-        yz /= lengthSquared;
+    const float zz = zps * zps;
-        yw /= lengthSquared;
+    const float zw = zps * wps;
-        zz /= lengthSquared;
-        zw /= lengthSquared;
-    }
 
     *pitch = std::asin(-2.0f * (yz - xw));
     if (*pitch < M_PI_2) {