From dd4679459d82303a3545f76bf648a64726021d87 Mon Sep 17 00:00:00 2001
From: Mateusz Pusz <mateusz.pusz@gmail.com>
Date: Wed, 3 Aug 2022 12:29:49 +0200
Subject: [PATCH] feat: `hypot` support added

---
 .../glide_computer/include/glide_computer.h   |  8 ++--
 src/core/include/units/math.h                 | 46 +++++++++++++++++++
 2 files changed, 49 insertions(+), 5 deletions(-)

diff --git a/example/glide_computer/include/glide_computer.h b/example/glide_computer/include/glide_computer.h
index c515868c..c846b72d 100644
--- a/example/glide_computer/include/glide_computer.h
+++ b/example/glide_computer/include/glide_computer.h
@@ -157,9 +157,8 @@ public:
   using legs = std::vector<leg>;
 
   template<std::ranges::input_range R>
-    requires std::same_as<std::ranges::range_value_t<R>,
-                          waypoint> explicit task(const R& r) :
-      waypoints_(std::ranges::begin(r), std::ranges::end(r))
+    requires std::same_as<std::ranges::range_value_t<R>, waypoint>
+  explicit task(const R& r) : waypoints_(std::ranges::begin(r), std::ranges::end(r))
   {
   }
 
@@ -215,8 +214,7 @@ constexpr height agl(altitude glider_alt, altitude terrain_level) { return glide
 
 inline units::isq::si::length<units::isq::si::kilometre> length_3d(distance dist, height h)
 {
-  // TODO support for hypot(q, q)
-  return sqrt(pow<2>(dist.common()) + pow<2>(h.common()));
+  return hypot(dist.common(), h.common());
 }
 
 distance glide_distance(const flight_point& pos, const glider& g, const task& t, const safety& s, altitude ground_alt);
diff --git a/src/core/include/units/math.h b/src/core/include/units/math.h
index 0de45139..2e9e7027 100644
--- a/src/core/include/units/math.h
+++ b/src/core/include/units/math.h
@@ -301,4 +301,50 @@ template<Quantity To, std::same_as<typename To::dimension> D, typename U, std::s
   return ::units::round<typename To::unit>(q);
 }
 
+/**
+ * @brief Computes the square root of the sum of the squares of x and y,
+ *        without undue overflow or underflow at intermediate stages of the computation
+ */
+template<Quantity Q1, Quantity Q2>
+[[nodiscard]] inline std::common_type_t<Q1, Q2> hypot(const Q1& x, const Q2& y) noexcept
+  requires requires { typename std::common_type_t<Q1, Q2>; } &&
+           requires(std::common_type_t<Q1, Q2> q) {
+             pow<2>(x);
+             pow<2>(y);
+             sqrt(pow<2>(x) + pow<2>(y));
+             requires requires { hypot(q.number(), q.number()); } || requires { std::hypot(q.number(), q.number()); };
+           }
+{
+  using type = std::common_type_t<Q1, Q2>;
+  type xx = x;
+  type yy = y;
+  using std::hypot;
+  return type(hypot(xx.number(), yy.number()));
+}
+
+/**
+ * @brief Computes the square root of the sum of the squares of x, y, and z,
+ *        without undue overflow or underflow at intermediate stages of the computation
+ */
+template<Quantity Q1, Quantity Q2, Quantity Q3>
+[[nodiscard]] inline std::common_type_t<std::common_type_t<Q1, Q2>, Q3> hypot(const Q1& x, const Q2& y,
+                                                                              const Q3& z) noexcept
+  requires requires { typename std::common_type_t<std::common_type_t<Q1, Q2>, Q3>; } &&
+           requires(std::common_type_t<std::common_type_t<Q1, Q2>, Q3> q) {
+             pow<2>(x);
+             pow<2>(y);
+             pow<2>(z);
+             sqrt(pow<2>(x) + pow<2>(y) + pow<2>(z));
+             requires requires { hypot(q.number(), q.number(), q.number()); } ||
+                        requires { std::hypot(q.number(), q.number(), q.number()); };
+           }
+{
+  using type = std::common_type_t<std::common_type_t<Q1, Q2>, Q3>;
+  type xx = x;
+  type yy = y;
+  type zz = z;
+  using std::hypot;
+  return type(hypot(xx.number(), yy.number(), zz.number()));
+}
+
 }  // namespace units