Optimize vector normalize for vectors of length 1

If the square of the length is very close to 1, then avoid the qSqrt().

Reviewed-by: trustme

5 files changed, 63 insertions, 39 deletions

diff --git a/src/gui/math3d/qmatrix4x4.cpp b/src/gui/math3d/qmatrix4x4.cpp
index e00d772..649532d 100644
--- a/src/gui/math3d/qmatrix4x4.cpp
+++ b/src/gui/math3d/qmatrix4x4.cpp
@@ -995,8 +995,9 @@ QMatrix4x4& QMatrix4x4::rotate(qreal angle, qreal x, qreal y, qreal z)
         quick = true;
     }
     if (!quick) {
-        qreal len = qSqrt(x * x + y * y + z * z);
-        if (len != 0) {
+        qreal len = x * x + y * y + z * z;
+        if (!qFuzzyIsNull(len - 1.0f) && !qFuzzyIsNull(len)) {
+            len = qSqrt(len);
             x /= len;
             y /= len;
             z /= len;
diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp
index a91b0b9..96659ea 100644
--- a/src/gui/math3d/qquaternion.cpp
+++ b/src/gui/math3d/qquaternion.cpp
@@ -230,40 +230,46 @@ qreal QQuaternion::lengthSquared() const
 }
 
 /*!
-    Returns the normalized unit form of this quaternion.  If this quaternion
-    is not null, the returned quaternion is guaranteed to be 1.0 in length.
+    Returns the normalized unit form of this quaternion.
+
     If this quaternion is null, then a null quaternion is returned.
+    If the length of the quaternion is very close to 1, then the quaternion
+    will be returned as-is.  Otherwise the normalized form of the
+    quaternion of length 1 will be returned.
 
     \sa length(), normalize()
 */
 QQuaternion QQuaternion::normalized() const
 {
-    qreal len = length();
-    if (!qIsNull(len))
-        return *this / len;
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f))
+        return *this;
+    else if (!qFuzzyIsNull(len))
+        return *this / qSqrt(len);
     else
         return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f);
 }
 
 /*!
     Normalizes the currect quaternion in place.  Nothing happens if this
-    is a null quaternion.
+    is a null quaternion or the length of the quaternion is very close to 1.
 
     \sa length(), normalized()
 */
 void QQuaternion::normalize()
 {
-    qreal len = length();
-    if (qIsNull(len))
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
         return;
 
+    len = qSqrt(len);
+
     xp /= len;
     yp /= len;
     zp /= len;
     wp /= len;
 }
 
-
 /*!
     \fn QQuaternion QQuaternion::conjugate() const
 
diff --git a/src/gui/math3d/qvector2d.cpp b/src/gui/math3d/qvector2d.cpp
index 54e1712..c3aaa42 100644
--- a/src/gui/math3d/qvector2d.cpp
+++ b/src/gui/math3d/qvector2d.cpp
@@ -179,33 +179,39 @@ qreal QVector2D::lengthSquared() const
 }
 
 /*!
-    Returns the normalized unit vector form of this vector.  If this vector
-    is not null, the returned vector is guaranteed to be 1.0 in length.
-    If this vector is null, then a null vector is returned.
+    Returns the normalized unit vector form of this vector.
+
+    If this vector is null, then a null vector is returned.  If the length
+    of the vector is very close to 1, then the vector will be returned as-is.
+    Otherwise the normalized form of the vector of length 1 will be returned.
 
     \sa length(), normalize()
 */
 QVector2D QVector2D::normalized() const
 {
-    qreal len = length();
-    if (!qIsNull(len))
-        return *this / len;
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f))
+        return *this;
+    else if (!qFuzzyIsNull(len))
+        return *this / qSqrt(len);
     else
         return QVector2D();
 }
 
 /*!
     Normalizes the currect vector in place.  Nothing happens if this
-    vector is a null vector.
+    vector is a null vector or the length of the vector is very close to 1.
 
     \sa length(), normalized()
 */
 void QVector2D::normalize()
 {
-    qreal len = length();
-    if (qIsNull(len))
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
         return;
 
+    len = qSqrt(len);
+
     xp /= len;
     yp /= len;
 }
diff --git a/src/gui/math3d/qvector3d.cpp b/src/gui/math3d/qvector3d.cpp
index 0613aa8..c83cd60 100644
--- a/src/gui/math3d/qvector3d.cpp
+++ b/src/gui/math3d/qvector3d.cpp
@@ -195,33 +195,39 @@ QVector3D::QVector3D(const QVector4D& vector)
 */
 
 /*!
-    Returns the normalized unit vector form of this vector.  If this vector
-    is not null, the returned vector is guaranteed to be 1.0 in length.
-    If this vector is null, then a null vector is returned.
+    Returns the normalized unit vector form of this vector.
+
+    If this vector is null, then a null vector is returned.  If the length
+    of the vector is very close to 1, then the vector will be returned as-is.
+    Otherwise the normalized form of the vector of length 1 will be returned.
 
     \sa length(), normalize()
 */
 QVector3D QVector3D::normalized() const
 {
-    qreal len = length();
-    if (!qIsNull(len))
-        return *this / len;
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f))
+        return *this;
+    else if (!qFuzzyIsNull(len))
+        return *this / qSqrt(len);
     else
         return QVector3D();
 }
 
 /*!
     Normalizes the currect vector in place.  Nothing happens if this
-    vector is a null vector.
+    vector is a null vector or the length of the vector is very close to 1.
 
     \sa length(), normalized()
 */
 void QVector3D::normalize()
 {
-    qreal len = length();
-    if (qIsNull(len))
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
         return;
 
+    len = qSqrt(len);
+
     xp /= len;
     yp /= len;
     zp /= len;
diff --git a/src/gui/math3d/qvector4d.cpp b/src/gui/math3d/qvector4d.cpp
index 8fafbfd..010fa53 100644
--- a/src/gui/math3d/qvector4d.cpp
+++ b/src/gui/math3d/qvector4d.cpp
@@ -247,40 +247,45 @@ qreal QVector4D::lengthSquared() const
 }
 
 /*!
-    Returns the normalized unit vector form of this vector.  If this vector
-    is not null, the returned vector is guaranteed to be 1.0 in length.
-    If this vector is null, then a null vector is returned.
+    Returns the normalized unit vector form of this vector.
+
+    If this vector is null, then a null vector is returned.  If the length
+    of the vector is very close to 1, then the vector will be returned as-is.
+    Otherwise the normalized form of the vector of length 1 will be returned.
 
     \sa length(), normalize()
 */
 QVector4D QVector4D::normalized() const
 {
-    qreal len = length();
-    if (!qIsNull(len))
-        return *this / len;
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f))
+        return *this;
+    else if (!qFuzzyIsNull(len))
+        return *this / qSqrt(len);
     else
         return QVector4D();
 }
 
 /*!
     Normalizes the currect vector in place.  Nothing happens if this
-    vector is a null vector.
+    vector is a null vector or the length of the vector is very close to 1.
 
     \sa length(), normalized()
 */
 void QVector4D::normalize()
 {
-    qreal len = length();
-    if (qIsNull(len))
+    qreal len = lengthSquared();
+    if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
         return;
 
+    len = qSqrt(len);
+
     xp /= len;
     yp /= len;
     zp /= len;
     wp /= len;
 }
 
-
 /*!
     \fn QVector4D &QVector4D::operator+=(const QVector4D &vector)