Support lerp Jet

Provide autodiff support for C++20 std::lerp if the standard is active.

Change-Id: I04ef6f17c707dd5f8ac426d9127b221e17aa08d6

include/ceres/internal/port.h

diff --git a/include/ceres/internal/port.h b/include/ceres/internal/port.h
index 620f13a..7c72bb5 100644
--- a/include/ceres/internal/port.h
+++ b/include/ceres/internal/port.h
@@ -111,4 +111,20 @@
 #define CERES_GET_FLAG(X) X
 #endif
 
+// Indicates whether C++17 is currently active
+#ifndef CERES_HAS_CPP17
+#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#define CERES_HAS_CPP17
+#endif  // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+        // 201703L)
+#endif  // !defined(CERES_HAS_CPP17)
+
+// Indicates whether C++20 is currently active
+#ifndef CERES_HAS_CPP20
+#if __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L)
+#define CERES_HAS_CPP20
+#endif  // __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+        // 202002L)
+#endif  // !defined(CERES_HAS_CPP20)
+
 #endif  // CERES_PUBLIC_INTERNAL_PORT_H_

include/ceres/jet.h

diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index d20038b..f6789d8 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -162,6 +162,7 @@
 #include <iosfwd>
 #include <iostream>  // NOLINT
 #include <limits>
+#include <numeric>
 #include <string>
 
 #include "Eigen/Core"
@@ -415,6 +416,10 @@
 using std::tan;
 using std::tanh;
 
+#ifdef CERES_HAS_CPP20
+using std::lerp;
+#endif  // defined(CERES_HAS_CPP20)
+
 // Legacy names from pre-C++11 days.
 // clang-format off
 inline bool IsFinite(double x)   { return std::isfinite(x); }
@@ -630,7 +635,7 @@
   return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
 }
 
-#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#ifdef CERES_HAS_CPP17
 // Like sqrt(x^2 + y^2 + z^2),
 // but acts to prevent underflow/overflow for small/large x/y/z.
 // Note that the function is non-smooth at x=y=z=0,
@@ -650,8 +655,7 @@
   const T tmp = hypot(x.a, y.a, z.a);
   return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v + z.a / tmp * z.v);
 }
-#endif  // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >=
-        // 201703L)
+#endif  // defined(CERES_HAS_CPP17)
 
 template <typename T, int N>
 inline Jet<T, N> fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
@@ -841,6 +845,30 @@
   return isinf(f);
 }
 
+#ifdef CERES_HAS_CPP20
+// Computes the linear interpolation a + t(b - a) between a and b at the value
+// t. For arguments outside of the range 0 <= t <= 1, the values are
+// extrapolated.
+//
+// Differentiating lerp(a, b, t) with respect to a, b, and t gives:
+//
+//   d/da lerp(a, b, t) = 1 - t
+//   d/db lerp(a, b, t) = t
+//   d/dt lerp(a, b, t) = b - a
+//
+// with the dual representation given by
+//
+//   lerp(a + da, b + db, t + dt)
+//      ~= lerp(a, b, t) + (1 - t) da + t db + (b - a) dt .
+template <typename T, int N>
+inline Jet<T, N> lerp(const Jet<T, N>& a,
+                      const Jet<T, N>& b,
+                      const Jet<T, N>& t) {
+  return Jet<T, N>{lerp(a.a, b.a, t.a),
+                   (T(1) - t.a) * a.v + t.a * b.v + (b.a - a.a) * t.v};
+}
+#endif  // defined(CERES_HAS_CPP20)
+
 // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
 //
 // In words: the rate of change of theta is 1/r times the rate of

internal/ceres/jet_test.cc

diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc
index 55a14b3..743405b 100644
--- a/internal/ceres/jet_test.cc
+++ b/internal/ceres/jet_test.cc
@@ -810,7 +810,7 @@
   NumericalTest2("hypot2", hypot2,  1.0,   2.0);
   // clang-format on
 
-#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#ifdef CERES_HAS_CPP17
   {  // Check that hypot(x, y) == sqrt(x^2 + y^2)
     J h = hypot(x, y, z);
     J s = sqrt(x * x + y * y + z * z);
@@ -893,8 +893,57 @@
     VL << "h = " << h;
     ExpectJetsClose(h, huge);
   }
-#endif  // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >=
-        // 201703L)
+#endif  // defined(CERES_HAS_CPP17)
+
+#ifdef CERES_HAS_CPP20
+  {  // Check lerp(x, y, 0) == x
+    J z = lerp(x, y, J{0});
+    VL << "z = " << z;
+    ExpectJetsClose(z, x);
+  }
+
+  {  // Check lerp(x, y, 1) == y
+    J z = lerp(x, y, J{1});
+    VL << "z = " << z;
+    ExpectJetsClose(z, y);
+  }
+
+  {  // Check lerp(x, x, 1) == x
+    J z = lerp(x, x, J{1});
+    VL << "z = " << z;
+    ExpectJetsClose(z, x);
+  }
+
+  {  // Check lerp(y, y, 0) == y
+    J z = lerp(y, y, J{1});
+    VL << "z = " << z;
+    ExpectJetsClose(z, y);
+  }
+
+  {  // Check lerp(x, y, 0.5) == (x + y) / 2
+    J z = lerp(x, y, J{0.5});
+    J v = (x + y) / J{2};
+    VL << "z = " << z;
+    VL << "v = " << v;
+    ExpectJetsClose(z, v);
+  }
+
+  {  // Check lerp(x, y, 2) == 2y - x
+    J z = lerp(x, y, J{2});
+    J v = J{2} * y - x;
+    VL << "z = " << z;
+    VL << "v = " << v;
+    ExpectJetsClose(z, v);
+  }
+
+  {  // Check lerp(x, y, -2) == 3x - 2y
+    J z = lerp(x, y, -J{2});
+    J v = J{3} * x - J{2} * y;
+    VL << "z = " << z;
+    VL << "v = " << v;
+    ExpectJetsClose(z, v);
+  }
+#endif  // defined(CERES_HAS_CPP20)
 
   {
     J z = fmax(x, y);