Partially revert hypot arguments zero checks

Unfortunately, libc++'s 3-argument std::hypot implementation is
numerically unstable until LLVM 19.x. Therefore, checking the arguments
for zeros is insufficient since an underflow can still occur resulting
in a zero norm which requires another check. As such, division by zero
cannot be reliably avoided.

Change-Id: I189c8dc722aaec1ebc3ec8b1a177e1d8ac3b36db

include/ceres/rotation.h

diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index dc04a16..261d3a0 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -320,11 +320,10 @@
   const T& a2 = angle_axis[2];
 
   T k;
+  const T theta = hypot(a0, a1, a2);
 
   // For points not at the origin, the full conversion is numerically stable.
-  if (fpclassify(a0) != FP_ZERO || fpclassify(a1) != FP_ZERO ||
-      fpclassify(a2) != FP_ZERO) {
-    const T theta = hypot(a0, a1, a2);
+  if (fpclassify(theta) != FP_ZERO) {
     const T half_theta = theta * T(0.5);
     k = sin(half_theta) / theta;
     quaternion[0] = cos(half_theta);
@@ -351,12 +350,11 @@
   const T& q3 = quaternion[3];
 
   T k;
+  const T sin_theta = hypot(q1, q2, q3);
 
   // For quaternions representing non-zero rotation, the conversion
   // is numerically stable.
-  if (fpclassify(q1) != FP_ZERO || fpclassify(q2) != FP_ZERO ||
-      fpclassify(q3) != FP_ZERO) {
-    const T sin_theta = hypot(q1, q2, q3);
+  if (fpclassify(sin_theta) != FP_ZERO) {
     const T& cos_theta = quaternion[0];
 
     // If cos_theta is negative, theta is greater than pi/2, which

internal/ceres/autodiff_manifold_test.cc

diff --git a/internal/ceres/autodiff_manifold_test.cc b/internal/ceres/autodiff_manifold_test.cc
index f4b6156..c7a473d 100644
--- a/internal/ceres/autodiff_manifold_test.cc
+++ b/internal/ceres/autodiff_manifold_test.cc
@@ -143,9 +143,8 @@
   template <typename T>
   bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
     T q_delta[4];
-    if (fpclassify(delta[0]) != FP_ZERO || fpclassify(delta[1]) != FP_ZERO ||
-        fpclassify(delta[2]) != FP_ZERO) {
-      T norm_delta = hypot(delta[0], delta[1], delta[2]);
+    T norm_delta = hypot(delta[0], delta[1], delta[2]);
+    if (fpclassify(norm_delta) != FP_ZERO) {
       const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
       q_delta[0] = cos(norm_delta);
       q_delta[1] = sin_delta_by_delta * delta[0];
@@ -171,11 +170,9 @@
     T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
     T ambient_y_minus_x[4];
     QuaternionProduct(y, minus_x, ambient_y_minus_x);
-    if (fpclassify(ambient_y_minus_x[1]) != FP_ZERO ||
-        fpclassify(ambient_y_minus_x[2]) != FP_ZERO ||
-        fpclassify(ambient_y_minus_x[3]) != FP_ZERO) {
-      T u_norm = hypot(
-          ambient_y_minus_x[1], ambient_y_minus_x[2], ambient_y_minus_x[3]);
+    T u_norm =
+        hypot(ambient_y_minus_x[1], ambient_y_minus_x[2], ambient_y_minus_x[3]);
+    if (fpclassify(u_norm) != FP_ZERO) {
       T theta = atan2(u_norm, ambient_y_minus_x[0]);
       y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
       y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;