From 5a0e350be754698fafef0d61d78c2f5ed9d8c503 Mon Sep 17 00:00:00 2001
From: Mateusz Pusz <mateusz.pusz@gmail.com>
Date: Thu, 13 Feb 2025 14:26:51 +0100
Subject: [PATCH] feat: `lerp` and `midpoint` for points added

---
 src/core/include/mp-units/math.h | 42 +++++++++++++++++
 test/runtime/math_test.cpp       | 77 ++++++++++++++++++++++++++++++++
 2 files changed, 119 insertions(+)

diff --git a/src/core/include/mp-units/math.h b/src/core/include/mp-units/math.h
index 1e51286a..efb6ad59 100644
--- a/src/core/include/mp-units/math.h
+++ b/src/core/include/mp-units/math.h
@@ -454,6 +454,48 @@ template<auto R1, typename Rep1, auto R2, typename Rep2, auto R3, typename Rep3>
   return quantity{hypot(x.numerical_value_in(unit), y.numerical_value_in(unit), z.numerical_value_in(unit)), ref};
 }
 
+/**
+ * @brief Linear interpolation or extrapolation
+ *
+ * Computes the linear interpolation between `a` and `b`, if the parameter `t` is inside `[​0​, 1)`
+ * (the linear extrapolation otherwise), i.e. the result of `a + t(b − a)` with accounting for
+ * floating-point calculation imprecision.
+ */
+template<auto R1, auto Origin, typename Rep1, auto R2, typename Rep2, typename Factor>
+  requires requires(Rep1 a, Rep2 b, Factor t) {
+    get_common_reference(R1, R2);
+    requires requires { lerp(a, b, t); } || requires { std::lerp(a, b, t); };
+  }
+[[nodiscard]] constexpr QuantityPointOf<get_quantity_spec(get_common_reference(R1, R2))> auto lerp(
+  const quantity_point<R1, Origin, Rep1>& a, const quantity_point<R2, Origin, Rep2>& b, const Factor& t) noexcept
+{
+  constexpr auto ref = get_common_reference(R1, R2);
+  constexpr auto unit = get_unit(ref);
+  using std::lerp;
+  return Origin + quantity{lerp(a.quantity_ref_from(Origin).numerical_value_in(unit),
+                                b.quantity_ref_from(Origin).numerical_value_in(unit), t),
+                           ref};
+}
+
+/**
+ * @brief Computes the midpoint of two points
+ */
+template<auto R1, auto Origin, typename Rep1, auto R2, typename Rep2>
+  requires requires(Rep1 a, Rep2 b) {
+    get_common_reference(R1, R2);
+    requires requires { midpoint(a, b); } || requires { std::midpoint(a, b); };
+  }
+[[nodiscard]] constexpr QuantityPointOf<get_quantity_spec(get_common_reference(R1, R2))> auto midpoint(
+  const quantity_point<R1, Origin, Rep1>& a, const quantity_point<R2, Origin, Rep2>& b) noexcept
+{
+  constexpr auto ref = get_common_reference(R1, R2);
+  constexpr auto unit = get_unit(ref);
+  using std::midpoint;
+  return Origin + quantity{midpoint(a.quantity_ref_from(Origin).numerical_value_in(unit),
+                                    b.quantity_ref_from(Origin).numerical_value_in(unit)),
+                           ref};
+}
+
 #endif  // MP_UNITS_HOSTED
 
 }  // namespace mp_units
diff --git a/test/runtime/math_test.cpp b/test/runtime/math_test.cpp
index 285eae00..e6416d1d 100644
--- a/test/runtime/math_test.cpp
+++ b/test/runtime/math_test.cpp
@@ -41,6 +41,9 @@ import mp_units;
 using namespace mp_units;
 using namespace mp_units::si::unit_symbols;
 
+inline constexpr struct mean_sea_level final : mp_units::absolute_point_origin<mp_units::isq::altitude> {
+} mean_sea_level;
+
 // classical
 
 TEST_CASE("math operations", "[math]")
@@ -353,6 +356,80 @@ TEST_CASE("math operations", "[math]")
     }
   }
 
+  SECTION("lerp functions")
+  {
+    SECTION("lerp should work on the same quantity points")
+    {
+      SECTION("default origins")
+      {
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[m]>(100.), 0.0) == point<isq::altitude[m]>(99.));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[m]>(100.), 0.5) ==
+                point<isq::altitude[m]>(99.5));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[m]>(100.), 1.0) ==
+                point<isq::altitude[m]>(100.));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[m]>(100.), 2.0) ==
+                point<isq::altitude[m]>(101.));
+      }
+
+      SECTION("custom origins")
+      {
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(100. * m), 0.0) ==
+                mean_sea_level + isq::height(99. * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(100. * m), 0.5) ==
+                mean_sea_level + isq::height(99.5 * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(100. * m), 1.0) ==
+                mean_sea_level + isq::height(100. * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(100. * m), 2.0) ==
+                mean_sea_level + isq::height(101. * m));
+      }
+    }
+
+    SECTION("lerp should work with different units of the same dimension")
+    {
+      SECTION("default origins")
+      {
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[cm]>(10'000.), 0.0) ==
+                point<isq::altitude[m]>(99.));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[cm]>(10'000.), 0.5) ==
+                point<isq::altitude[m]>(99.5));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[cm]>(10'000.), 1.0) ==
+                point<isq::altitude[m]>(100.));
+        REQUIRE(lerp(point<isq::altitude[m]>(99.), point<isq::altitude[cm]>(10'000.), 2.0) ==
+                point<isq::altitude[m]>(101.));
+      }
+
+      SECTION("custom origins")
+      {
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(10'000. * cm), 0.0) ==
+                mean_sea_level + isq::height(99. * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(10'000. * cm), 0.5) ==
+                mean_sea_level + isq::height(99.5 * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(10'000. * cm), 1.0) ==
+                mean_sea_level + isq::height(100. * m));
+        REQUIRE(lerp(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(10'000. * cm), 2.0) ==
+                mean_sea_level + isq::height(101. * m));
+      }
+    }
+  }
+
+  SECTION("midpoint functions")
+  {
+    SECTION("midpoint should work on the same quantity points")
+    {
+      REQUIRE(midpoint(point<isq::altitude[m]>(99.), point<isq::altitude[m]>(100.)) == point<isq::altitude[m]>(99.5));
+      REQUIRE(midpoint(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(100. * m)) ==
+              mean_sea_level + isq::height(99.5 * m));
+    }
+
+    SECTION("midpoint should work with different units of the same dimension")
+    {
+      REQUIRE(midpoint(point<isq::altitude[m]>(99.), point<isq::altitude[cm]>(10'000.)) ==
+              point<isq::altitude[m]>(99.5));
+      REQUIRE(midpoint(mean_sea_level + isq::height(99. * m), mean_sea_level + isq::height(10'000. * cm)) ==
+              mean_sea_level + isq::height(99.5 * m));
+    }
+  }
+
   SECTION("SI trigonometric functions")
   {
     SECTION("sin")