Add qHypot() to qmath.h, exposing and extending std::hypot()

We have plenty of places where we add some squares and take a square
root; this may be done more accurately and faster by hypot().
Introduce QHypotHelper to handle hypot with more than 3 parameters,
and with 3 when the C++17 version is missing (which it never should
be). Include an overload taking arbitrarily many valus and ensure that
we can use qHypot() with qfloat16.  Illustrate with some example uses,
add some tests.

[ChangeLog][QtCore][QMath] Header <QMath> now provides qHypot(), an
implementation of std::hypot() taking arbitrarily many numeric values,
including support for qfloat16, while avoiding the overflow and
underflow problems that arise when naively taking the square root of a
sum of squares.

Change-Id: Ia4e3913fe83fc27d17d8e7f1a52f03ad445c1fed
Reviewed-by: Andrei Golubev <andrei.golubev@qt.io>
Reviewed-by: Mårten Nordheim <marten.nordheim@qt.io>

4 files changed, 183 insertions, 12 deletions

diff --git a/src/corelib/global/qfloat16.h b/src/corelib/global/qfloat16.h
index a25fac2886..387735863e 100644
--- a/src/corelib/global/qfloat16.h
+++ b/src/corelib/global/qfloat16.h
@@ -1,6 +1,6 @@
 /****************************************************************************
 **
-** Copyright (C) 2020 The Qt Company Ltd.
+** Copyright (C) 2021 The Qt Company Ltd.
 ** Copyright (C) 2016 by Southwest Research Institute (R)
 ** Contact: http://www.qt-project.org/legal
 **
@@ -308,6 +308,63 @@ inline qfloat16::operator float() const noexcept
     return qAbs(static_cast<float>(f)) <= 0.001f;
 }
 
+/*
+  qHypot compatibility; see ../kernel/qmath.h
+*/
+namespace QtPrivate {
+template <typename R>
+struct QHypotType<R, qfloat16> { using type = decltype(std::hypot(R(1), 1.0f)); };
+template <typename R>
+struct QHypotType<qfloat16, R> { using type = decltype(std::hypot(1.0f, R(1))); };
+template <> struct QHypotType<qfloat16, qfloat16> { using type = qfloat16; };
+}
+// Avoid passing qfloat16 to std::hypot(), while ensuring return types
+// consistent with the above:
+template<typename F, typename ...Fs> auto qHypot(F first, Fs... rest);
+template <typename T, typename std::enable_if<!std::is_same<qfloat16, T>::value, int>::type = 0>
+auto qHypot(T x, qfloat16 y) { return qHypot(x, float(y)); }
+template <typename T, typename std::enable_if<!std::is_same<qfloat16, T>::value, int>::type = 0>
+auto qHypot(qfloat16 x, T y) { return qHypot(float(x), y); }
+template <> inline auto qHypot(qfloat16 x, qfloat16 y)
+{
+#if (defined(QT_COMPILER_SUPPORTS_F16C) && defined(__F16C__)) || defined (__ARM_FP16_FORMAT_IEEE)
+    return QtPrivate::QHypotHelper<qfloat16>(x).add(y).result();
+#else
+    return qfloat16(qHypot(float(x), float(y)));
+#endif
+}
+#if __cpp_lib_hypot >= 201603L // Expected to be true
+// If any are not qfloat16, convert each qfloat16 to float:
+/* (The following splits the some-but-not-all-qfloat16 cases up, using
+   (X|Y|Z)&~(X&Y&Z) = X ? ~(Y&Z) : Y|Z = X&~(Y&Z) | ~X&Y | ~X&~Y&Z,
+   into non-overlapping cases, to avoid ambiguity.) */
+template <typename Ty, typename Tz,
+          typename std::enable_if<
+              // Ty, Tz aren't both qfloat16:
+              !(std::is_same_v<qfloat16, Ty> && std::is_same_v<qfloat16, Tz>), int>::type = 0>
+auto qHypot(qfloat16 x, Ty y, Tz z) { return qHypot(float(x), y, z); }
+template <typename Tx, typename Tz,
+          typename std::enable_if<
+              // Tx isn't qfloat16:
+              !std::is_same_v<qfloat16, Tx>, int>::type = 0>
+auto qHypot(Tx x, qfloat16 y, Tz z) { return qHypot(x, float(y), z); }
+template <typename Tx, typename Ty,
+          typename std::enable_if<
+              // Neither Tx nor Ty is qfloat16:
+              !std::is_same_v<qfloat16, Tx> && !std::is_same_v<qfloat16, Ty>, int>::type = 0>
+auto qHypot(Tx x, Ty y, qfloat16 z) { return qHypot(x, y, float(z)); }
+// If all are qfloat16, stay with qfloat16 (albeit via float, if no native support):
+template <>
+inline auto qHypot(qfloat16 x, qfloat16 y, qfloat16 z)
+{
+#if (defined(QT_COMPILER_SUPPORTS_F16C) && defined(__F16C__)) || defined (__ARM_FP16_FORMAT_IEEE)
+    return QtPrivate::QHypotHelper<qfloat16>(x).add(y).add(z).result();
+#else
+    return qfloat16(qHypot(float(x), float(y), float(z)));
+#endif
+}
+#endif // 3-arg std::hypot() is available
+
 QT_END_NAMESPACE
 
 Q_DECLARE_METATYPE(qfloat16)
diff --git a/src/corelib/kernel/qmath.h b/src/corelib/kernel/qmath.h
index 176e7d7978..298bb630fb 100644
--- a/src/corelib/kernel/qmath.h
+++ b/src/corelib/kernel/qmath.h
@@ -1,6 +1,6 @@
 /****************************************************************************
 **
-** Copyright (C) 2016 The Qt Company Ltd.
+** Copyright (C) 2021 The Qt Company Ltd.
 ** Contact: https://www.qt.io/licensing/
 **
 ** This file is part of the QtCore module of the Qt Toolkit.
@@ -135,6 +135,72 @@ template <typename T> auto qSqrt(T v)
     return sqrt(v);
 }
 
+namespace QtPrivate {
+template <typename R, typename F> // For qfloat16 to specialize
+struct QHypotType { using type = decltype(std::hypot(R(1), F(1))); };
+
+// Implements hypot() without limiting number of arguments:
+template <typename T>
+class QHypotHelper
+{
+    const T scale, total;
+    template <typename F> friend class QHypotHelper;
+    QHypotHelper(T first, T prior) : scale(first), total(prior) {}
+public:
+    QHypotHelper(T first) : scale(qAbs(first)), total(1) {}
+    T result() const
+    { return qIsFinite(scale) ? scale > 0 ? scale * T(std::sqrt(total)) : T(0) : scale; }
+
+    template<typename F, typename ...Fs>
+    auto add(F first, Fs... rest) const
+    { return add(first).add(rest...); }
+
+    template<typename F, typename R = typename QHypotType<T, F>::type>
+    QHypotHelper<R> add(F next) const
+    {
+        if (qIsInf(scale) || (qIsNaN(scale) && !qIsInf(next)))
+            return QHypotHelper<R>(scale, R(1));
+        if (qIsNaN(next))
+            return QHypotHelper<R>(next, R(1));
+        const R val = qAbs(next);
+        if (!(scale > 0) || qIsInf(next))
+            return QHypotHelper<R>(val, R(1));
+        if (!(val > 0))
+            return QHypotHelper<R>(scale, total);
+        if (val > scale) {
+            const R ratio = scale / next;
+            return QHypotHelper<R>(val, total * ratio * ratio + 1);
+        }
+        const R ratio = next / scale;
+        return QHypotHelper<R>(scale, total + ratio * ratio);
+    }
+};
+} // QtPrivate
+
+template<typename F, typename ...Fs>
+auto qHypot(F first, Fs... rest)
+{
+    return QtPrivate::QHypotHelper<F>(first).add(rest...).result();
+}
+
+// However, where possible, use the standard library implementations:
+template <typename Tx, typename Ty>
+auto qHypot(Tx x, Ty y)
+{
+    // C99 has hypot(), hence C++11 has std::hypot()
+    using std::hypot;
+    return hypot(x, y);
+}
+
+#if __cpp_lib_hypot >= 201603L // Expected to be true
+template <typename Tx, typename Ty, typename Tz>
+auto qHypot(Tx x, Ty y, Tz z)
+{
+    using std::hypot;
+    return hypot(x, y, z);
+}
+#endif // else: no need to over-ride the arbitrarily-many-arg form
+
 template <typename T> auto qLn(T v)
 {
     using std::log;
diff --git a/src/corelib/kernel/qmath.qdoc b/src/corelib/kernel/qmath.qdoc
index b0a52a4a52..13655bcc11 100644
--- a/src/corelib/kernel/qmath.qdoc
+++ b/src/corelib/kernel/qmath.qdoc
@@ -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 documentation of the Qt Toolkit.
@@ -139,7 +139,7 @@
     This function will return the angle (argument) of that point.
 
     \relates <QtMath>
-    \sa qAtan()
+    \sa qAtan(), qHypot()
 */
 
 /*!
@@ -148,7 +148,57 @@
     This function returns a NaN if \a v is a negative number.
 
     \relates <QtMath>
-    \sa qPow()
+    \sa qPow(), qHypot()
+*/
+
+/*!
+    \since 6.1
+    \overload
+    \fn template <typename Tx, typename Ty> auto qHypot(Tx x, Ty y)
+    Returns the distance of a point (x, y) from the origin (0, 0).
+
+    This is qSqrt(x * x + y * y), optimized.
+    In particular, underflow and overflow may be avoided.
+
+    Accepts any mix of numeric types, returning the same
+    floating-point type as std::hypot(). If either parameter is
+    infinite, so is the result; otherwise, if either is a NaN, so is
+    the result.
+
+    \relates <QtMath>
+    \sa qSqrt(), qAtan2()
+*/
+
+/*!
+    \since 6.1
+    \overload
+    \fn template <typename Tx, typename Ty, typename Tz> auto qHypot(Tx x, Ty y, Tz z)
+    Returns the distance of a point (x, y, z) from the origin (0, 0, 0).
+
+    This is qSqrt(x * x + y * y + z * z), optimized where supported.
+    In particular, underflow and overflow may be avoided.
+
+    Accepts any mix of numeric types, returning the same
+    floating-point type as std::hypot(). If any parameter is infinite,
+    so is the result; otherwise, if any is NaN, so is the result.
+
+    \relates <QtMath>
+    \sa qSqrt()
+*/
+
+/*!
+    \since 6.1
+    \fn template<typename F, typename ...Fs> auto qHypot(F first, Fs... rest)
+    Returns the distance from origin in arbitrarily many dimensions
+
+    This is as for the two-argument and three-argument forms, supported by
+    std::hypot(), but with as many numeric parameters as you care to pass to
+    it. Uses \a first and each of the \a rest as co-ordinates, performing a
+    calculation equivalent to squaring each, summing and returning the square
+    root, save that underflow and overflow are avoided as far as possible.
+
+    \relates <QtMath>
+    \sa qSqrt()
 */
 
 /*!
diff --git a/src/corelib/tools/qline.cpp b/src/corelib/tools/qline.cpp
index 1eb645501c..dc0a6c72fb 100644
--- a/src/corelib/tools/qline.cpp
+++ b/src/corelib/tools/qline.cpp
@@ -572,8 +572,7 @@ QDataStream &operator>>(QDataStream &stream, QLine &line)
 */
 qreal QLineF::length() const
 {
-    using std::hypot;
-    return hypot(dx(), dy());
+    return qHypot(dx(), dy());
 }
 
 /*!
@@ -651,12 +650,11 @@ QLineF QLineF::fromPolar(qreal length, qreal angle)
 */
 QLineF QLineF::unitVector() const
 {
-    qreal x = dx();
-    qreal y = dy();
-    using std::hypot;
-    qreal len = hypot(x, y);
+    const qreal x = dx();
+    const qreal y = dy();
 
-    QLineF f(p1(), QPointF(pt1.x() + x/len, pt1.y() + y/len));
+    const qreal len = qHypot(x, y);
+    QLineF f(p1(), QPointF(pt1.x() + x / len, pt1.y() + y / len));
 
 #ifndef QT_NO_DEBUG
     if (qAbs(f.length() - 1) >= 0.001)