support 3-argument hypot jet

C++17 provides a three argument hypot(x, y, z) which can now be used
for jets if the standard is active.

Change-Id: Ide62e101f780fe738bb2d4f826b10daf94c585b3

include/ceres/jet.h

diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index 38a69f4..d20038b 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -630,6 +630,29 @@
   return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
 }
 
+#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+// 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,
+// so the derivative is undefined there.
+template <typename T, int N>
+inline Jet<T, N> hypot(const Jet<T, N>& x,
+                       const Jet<T, N>& y,
+                       const Jet<T, N>& z) {
+  // d/da sqrt(a) = 0.5 / sqrt(a)
+  // d/dx x^2 + y^2 + z^2 = 2x
+  // So by the chain rule:
+  // d/dx sqrt(x^2 + y^2 + z^2)
+  //    = 0.5 / sqrt(x^2 + y^2 + z^2) * 2x
+  //    = x / sqrt(x^2 + y^2 + z^2)
+  // d/dy sqrt(x^2 + y^2 + z^2) = y / sqrt(x^2 + y^2 + z^2)
+  // d/dz sqrt(x^2 + y^2 + z^2) = z / sqrt(x^2 + y^2 + z^2)
+  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)
+
 template <typename T, int N>
 inline Jet<T, N> fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
   using std::isgreater;

internal/ceres/jet_test.cc

diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc
index b7b6004..55a14b3 100644
--- a/internal/ceres/jet_test.cc
+++ b/internal/ceres/jet_test.cc
@@ -116,9 +116,11 @@
   // Pick arbitrary values for x and y.
   J x = MakeJet(2.3, -2.7, 1e-3);
   J y = MakeJet(1.7, 0.5, 1e+2);
+  J z = MakeJet(1e-6, 1e-4, 1e-2);
 
   VL << "x = " << x;
   VL << "y = " << y;
+  VL << "z = " << z;
 
   {  // Check that log(exp(x)) == x.
     J z = exp(x);
@@ -792,18 +794,108 @@
     ExpectJetsClose(h, huge);
   }
 
+  // Resolve the ambiguity between two and three argument hypot overloads
+  using Hypot2 = J(const J&, const J&);
+  Hypot2* const hypot2 = static_cast<Hypot2*>(&hypot<double, 2>);
+
   // clang-format off
-  NumericalTest2("hypot", hypot<double, 2>,  0.0,   1e-5);
-  NumericalTest2("hypot", hypot<double, 2>, -1e-5,  0.0);
-  NumericalTest2("hypot", hypot<double, 2>,  1e-5,  1e-5);
-  NumericalTest2("hypot", hypot<double, 2>,  0.0,   1.0);
-  NumericalTest2("hypot", hypot<double, 2>,  1e-3,  1.0);
-  NumericalTest2("hypot", hypot<double, 2>,  1e-3, -1.0);
-  NumericalTest2("hypot", hypot<double, 2>, -1e-3,  1.0);
-  NumericalTest2("hypot", hypot<double, 2>, -1e-3, -1.0);
-  NumericalTest2("hypot", hypot<double, 2>,  1.0,   2.0);
+  NumericalTest2("hypot2", hypot2,  0.0,   1e-5);
+  NumericalTest2("hypot2", hypot2, -1e-5,  0.0);
+  NumericalTest2("hypot2", hypot2,  1e-5,  1e-5);
+  NumericalTest2("hypot2", hypot2,  0.0,   1.0);
+  NumericalTest2("hypot2", hypot2,  1e-3,  1.0);
+  NumericalTest2("hypot2", hypot2,  1e-3, -1.0);
+  NumericalTest2("hypot2", hypot2, -1e-3,  1.0);
+  NumericalTest2("hypot2", hypot2, -1e-3, -1.0);
+  NumericalTest2("hypot2", hypot2,  1.0,   2.0);
   // clang-format on
 
+#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+  {  // 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);
+    VL << "h = " << h;
+    VL << "s = " << s;
+    ExpectJetsClose(h, s);
+  }
+
+  {  // Check that hypot(x, x) == sqrt(3) * abs(x)
+    J h = hypot(x, x, x);
+    J s = sqrt(3.0) * abs(x);
+    VL << "h = " << h;
+    VL << "s = " << s;
+    ExpectJetsClose(h, s);
+  }
+
+  {  // Check that the derivative is zero tangentially to the circle:
+    J h = hypot(MakeJet(2.0, 1.0, 1.0),
+                MakeJet(2.0, 1.0, -1.0),
+                MakeJet(2.0, -1.0, 0.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, MakeJet(sqrt(12.0), 1.0 / std::sqrt(3.0), 0.0));
+  }
+
+  {  // Check that hypot(x, 0, 0) == x
+    J zero = MakeJet(0.0, 2.0, 3.14);
+    J h = hypot(x, zero, zero);
+    VL << "h = " << h;
+    ExpectJetsClose(x, h);
+  }
+
+  {  // Check that hypot(0, y, 0) == y
+    J zero = MakeJet(0.0, 2.0, 3.14);
+    J h = hypot(zero, y, zero);
+    VL << "h = " << h;
+    ExpectJetsClose(y, h);
+  }
+
+  {  // Check that hypot(0, 0, z) == z
+    J zero = MakeJet(0.0, 2.0, 3.14);
+    J h = hypot(zero, zero, z);
+    VL << "h = " << h;
+    ExpectJetsClose(z, h);
+  }
+
+  {  // Check that hypot(x, y, z) == hypot(hypot(x, y), z)
+    J v = hypot(x, y, z);
+    J w = hypot(hypot(x, y), z);
+    VL << "v = " << v;
+    VL << "w = " << w;
+    ExpectJetsClose(v, w);
+  }
+
+  {  // Check that hypot(x, y, z) == hypot(x, hypot(y, z))
+    J v = hypot(x, y, z);
+    J w = hypot(x, hypot(y, z));
+    VL << "v = " << v;
+    VL << "w = " << w;
+    ExpectJetsClose(v, w);
+  }
+
+  {  // Check that hypot(x, 0, 0) == sqrt(x * x) == x, even when x * x
+     // underflows:
+    EXPECT_EQ(
+        std::numeric_limits<double>::min() * std::numeric_limits<double>::min(),
+        0.0);  // Make sure it underflows
+    J huge = MakeJet(std::numeric_limits<double>::min(), 2.0, 3.14);
+    J h = hypot(huge, J(0.0), J(0.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, huge);
+  }
+
+  {  // Check that hypot(x, 0, 0) == sqrt(x * x) == x, even when x * x
+     // overflows:
+    EXPECT_EQ(
+        std::numeric_limits<double>::max() * std::numeric_limits<double>::max(),
+        std::numeric_limits<double>::infinity());
+    J huge = MakeJet(std::numeric_limits<double>::max(), 2.0, 3.14);
+    J h = hypot(huge, J(0.0), J(0.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, huge);
+  }
+#endif  // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+        // 201703L)
+
   {
     J z = fmax(x, y);
     VL << "z = " << z;