Use hypot to compute the L^2 norm

Change-Id: I908eaaa279452aa16346dfd3f25aac53685e3172
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index a92d84a..1220173 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2019 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
 // http://ceres-solver.org/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -47,7 +47,6 @@
 
 #include <algorithm>
 #include <cmath>
-#include <limits>
 
 #include "ceres/constants.h"
 #include "ceres/internal/euler_angles.h"
@@ -314,14 +313,15 @@
 
 template <typename T>
 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
+  using std::fpclassify;
+  using std::hypot;
   const T& a0 = angle_axis[0];
   const T& a1 = angle_axis[1];
   const T& a2 = angle_axis[2];
-  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
+  const T theta = hypot(a0, a1, a2);
 
   // For points not at the origin, the full conversion is numerically stable.
-  if (theta_squared > T(0.0)) {
-    const T theta = sqrt(theta_squared);
+  if (fpclassify(theta) != FP_ZERO) {
     const T half_theta = theta * T(0.5);
     const T k = sin(half_theta) / theta;
     quaternion[0] = cos(half_theta);
@@ -343,15 +343,16 @@
 
 template <typename T>
 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
+  using std::fpclassify;
+  using std::hypot;
   const T& q1 = quaternion[1];
   const T& q2 = quaternion[2];
   const T& q3 = quaternion[3];
-  const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
+  const T sin_theta = hypot(q1, q2, q3);
 
   // For quaternions representing non-zero rotation, the conversion
   // is numerically stable.
-  if (sin_squared_theta > T(0.0)) {
-    const T sin_theta = sqrt(sin_squared_theta);
+  if (fpclassify(sin_theta) != FP_ZERO) {
     const T& cos_theta = quaternion[0];
 
     // If cos_theta is negative, theta is greater than pi/2, which
@@ -452,13 +453,14 @@
 template <typename T, int row_stride, int col_stride>
 void AngleAxisToRotationMatrix(
     const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) {
+  using std::fpclassify;
+  using std::hypot;
   static const T kOne = T(1.0);
-  const T theta2 = DotProduct(angle_axis, angle_axis);
-  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+  const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
+  if (fpclassify(theta) != FP_ZERO) {
     // We want to be careful to only evaluate the square root if the
     // norm of the angle_axis vector is greater than zero. Otherwise
     // we get a division by zero.
-    const T theta = sqrt(theta2);
     const T wx = angle_axis[0] / theta;
     const T wy = angle_axis[1] / theta;
     const T wz = angle_axis[2] / theta;
@@ -478,7 +480,7 @@
     R(2, 2) =     costheta   + wz*wz*(kOne -    costheta);
     // clang-format on
   } else {
-    // Near zero, we switch to using the first order Taylor expansion.
+    // At zero, we switch to using the first order Taylor expansion.
     R(0, 0) = kOne;
     R(1, 0) = angle_axis[2];
     R(2, 0) = -angle_axis[1];
@@ -741,10 +743,10 @@
 template <typename T>
 inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
   DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
+  using std::hypot;
 
   // 'scale' is 1 / norm(q).
-  const T scale =
-      T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+  const T scale = T(1) / hypot(q[0], q[1], hypot(q[2], q[3]));
 
   // Make unit-norm version of q.
   const T unit[4] = {
@@ -791,9 +793,12 @@
                                  const T pt[3],
                                  T result[3]) {
   DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
+  using std::fpclassify;
+  using std::hypot;
 
-  const T theta2 = DotProduct(angle_axis, angle_axis);
-  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+  const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
+
+  if (fpclassify(theta) != FP_ZERO) {
     // Away from zero, use the rodriguez formula
     //
     //   result = pt costheta +
@@ -804,7 +809,6 @@
     // norm of the angle_axis vector is greater than zero. Otherwise
     // we get a division by zero.
     //
-    const T theta = sqrt(theta2);
     const T costheta = cos(theta);
     const T sintheta = sin(theta);
     const T theta_inverse = T(1.0) / theta;
@@ -825,7 +829,7 @@
     result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
     result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
   } else {
-    // Near zero, the first order Taylor approximation of the rotation
+    // At zero, the first order Taylor approximation of the rotation
     // matrix R corresponding to a vector w and angle theta is
     //
     //   R = I + hat(w) * sin(theta)
@@ -837,7 +841,7 @@
     // and actually performing multiplication with the point pt, gives us
     // R * pt = pt + angle_axis x pt.
     //
-    // Switching to the Taylor expansion near zero provides meaningful
+    // Switching to the Taylor expansion at zero provides meaningful
     // derivatives when evaluated using Jets.
     //
     // Explicitly inlined evaluation of the cross product for
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
index f412a79..a2e4c6d 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -30,13 +30,11 @@
                                double* x_plus_delta) {
   // x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the
   // quaternion constructed from delta.
-  const double norm_delta = std::sqrt(
-      delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
+  const double norm_delta = std::hypot(delta[0], delta[1], delta[2]);
 
-  if (norm_delta == 0.0) {
-    for (int i = 0; i < 4; ++i) {
-      x_plus_delta[i] = x[i];
-    }
+  if (std::fpclassify(norm_delta) == FP_ZERO) {
+    // No change in rotation: return the quaternion as is.
+    std::copy_n(x, 4, x_plus_delta);
     return;
   }
 
@@ -100,19 +98,16 @@
       -y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] +
       y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW];
 
-  const double u_norm =
-      std::sqrt(ambient_y_minus_x[Order::kX] * ambient_y_minus_x[Order::kX] +
-                ambient_y_minus_x[Order::kY] * ambient_y_minus_x[Order::kY] +
-                ambient_y_minus_x[Order::kZ] * ambient_y_minus_x[Order::kZ]);
-  if (u_norm > 0.0) {
+  const double u_norm = std::hypot(ambient_y_minus_x[Order::kX],
+                                   ambient_y_minus_x[Order::kY],
+                                   ambient_y_minus_x[Order::kZ]);
+  if (std::fpclassify(u_norm) != FP_ZERO) {
     const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
     y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm;
     y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm;
     y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm;
   } else {
-    y_minus_x[0] = 0.0;
-    y_minus_x[1] = 0.0;
-    y_minus_x[2] = 0.0;
+    std::fill_n(y_minus_x, 3, 0.0);
   }
 }
 
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 688edc3..577f8ed 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2022 Google Inc. All rights reserved.
+// Copyright 2023 Google Inc. All rights reserved.
 // http://ceres-solver.org/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -309,9 +309,7 @@
   quaternion[2] = 0.0;
   quaternion[3] = 0.0;
   QuaternionToAngleAxis(quaternion, angle_axis);
-  const double angle =
-      sqrt(angle_axis[0] * angle_axis[0] + angle_axis[1] * angle_axis[1] +
-           angle_axis[2] * angle_axis[2]);
+  const double angle = std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
   EXPECT_LE(angle, kPi);
 }
 
@@ -1196,14 +1194,13 @@
   for (int i = 0; i < 10000; ++i) {
     double theta = (2.0 * i * 0.0011 - 1.0) * kPi;
     for (int j = 0; j < 50; ++j) {
-      double norm2 = 0.0;
       for (int k = 0; k < 3; ++k) {
         angle_axis[k] = uniform_distribution(prng);
         p[k] = uniform_distribution(prng);
-        norm2 = angle_axis[k] * angle_axis[k];
       }
 
-      const double inv_norm = theta / sqrt(norm2);
+      const double inv_norm =
+          theta / std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
       for (double& angle_axi : angle_axis) {
         angle_axi *= inv_norm;
       }