Use fma to improve even further numerical stability

Author: mpuccio

Detectors/ITSMFT/common/tracking/src/Propagator.cxx

@@ -396,47 +396,49 @@ template <typename State>
 bool propagateHelixWithJacobian(State& state, float targetZ, float bz, DenseMatrix5& jacobian) noexcept
 {
   identity(jacobian);
-  const double dz = static_cast<double>(targetZ) - state.referenceCoordinate;
-  if (dz == 0.) {
+  const float dz = targetZ - state.referenceCoordinate;
+  if (dz == 0.f) {
     return true;
   }
-  const double tanl = state.parameters[3];
-  const double inverseQPt = state.parameters[4];
-  if (tanl == 0. || bz == 0.f || inverseQPt == 0.) {
+  const float tanl = state.parameters[3];
+  const float inverseQPt = state.parameters[4];
+  if (tanl == 0.f || bz == 0.f || inverseQPt == 0.f) {
     return false;
   }
-  const double n = dz / tanl;
-  const double curvatureScale = -std::abs(static_cast<double>(o2::constants::math::B2C)) * bz;
-  const double halfAnglePerQPt = 0.5 * curvatureScale * n;
-  const double halfAngle = inverseQPt * halfAnglePerQPt;
-  double sinc, sincDerivative;
-  if (std::abs(halfAngle) < 0.01) {
+  const float n = dz / tanl;
+  const float curvatureScale = -std::abs(o2::constants::math::B2C) * bz;
+  const float halfAnglePerQPt = 0.5f * curvatureScale * n;
+  const float halfAngle = inverseQPt * halfAnglePerQPt;
+  float sinc, sincDerivative;
+  if (std::abs(halfAngle) < 0.25f) {
     // sin(h)/h and its derivative, including their limits at h = 0.
-    const double h2 = halfAngle * halfAngle;
-    sinc = 1. + h2 * (-1. / 6. + h2 * (1. / 120. - h2 / 5040.));
-    sincDerivative = halfAngle * (-1. / 3. + h2 * (1. / 30. - h2 / 840.));
+    // Keep the cancellation-prone derivative quotient away from small h.
+    // At |h| <= 0.25 the omitted terms are below float precision.
+    const float h2 = halfAngle * halfAngle;
+    sinc = std::fma(h2, std::fma(h2, std::fma(h2, -1.f / 5040.f, 1.f / 120.f), -1.f / 6.f), 1.f);
+    sincDerivative = halfAngle * std::fma(h2, std::fma(h2, -1.f / 840.f, 1.f / 30.f), -1.f / 3.f);
   } else {
     sinc = std::sin(halfAngle) / halfAngle;
     sincDerivative = (std::cos(halfAngle) - sinc) / halfAngle;
   }
-  const double phi = state.parameters[2];
-  const double sinMid = std::sin(phi + halfAngle);
-  const double cosMid = std::cos(phi + halfAngle);
-  const double endPhi = phi + 2. * halfAngle;
-  const double dx = n * sinc * cosMid;
-  const double dy = n * sinc * sinMid;
+  const float phi = state.parameters[2];
+  const float sinMid = std::sin(phi + halfAngle);
+  const float cosMid = std::cos(phi + halfAngle);
+  const float endPhi = phi + 2.f * halfAngle;
+  const float dx = n * sinc * cosMid;
+  const float dy = n * sinc * sinMid;
 
   jacobian[0][2] = -dy;
   jacobian[1][2] = dx;
   jacobian[0][3] = -n / tanl * std::cos(endPhi);
   jacobian[1][3] = -n / tanl * std::sin(endPhi);
-  jacobian[0][4] = n * halfAnglePerQPt * (sincDerivative * cosMid - sinc * sinMid);
-  jacobian[1][4] = n * halfAnglePerQPt * (sincDerivative * sinMid + sinc * cosMid);
-  jacobian[2][3] = -2. * halfAngle / tanl;
-  jacobian[2][4] = 2. * halfAnglePerQPt;
+  jacobian[0][4] = n * halfAnglePerQPt * std::fma(sincDerivative, cosMid, -sinc * sinMid);
+  jacobian[1][4] = n * halfAnglePerQPt * std::fma(sincDerivative, sinMid, sinc * cosMid);
+  jacobian[2][3] = -2.f * halfAngle / tanl;
+  jacobian[2][4] = 2.f * halfAnglePerQPt;
 
-  state.parameters[0] += dx;
-  state.parameters[1] += dy;
+  state.parameters[0] = std::fma(n * sinc, cosMid, state.parameters[0]);
+  state.parameters[1] = std::fma(n * sinc, sinMid, state.parameters[1]);
   state.parameters[2] = endPhi;
   state.referenceCoordinate = targetZ;
   return true;

Detectors/ITSMFT/common/tracking/test/testPropagator.cxx

@@ -299,6 +299,73 @@ BOOST_AUTO_TEST_CASE(ForwardHelixSmallAngleMomentumDerivative)
   }
 }
 
+BOOST_AUTO_TEST_CASE(ForwardHelixFloatSeriesBoundary)
+{
+  // Isolate the q/pT Jacobian column with a unit momentum variance. An
+  // independent double-precision trajectory supplies numerical derivatives;
+  // checking only total position variances can hide this column's cancellation.
+  for (const float bz : {-5.f, 5.f}) {
+    for (const float halfAngle : {-0.5f, -0.2501f, -0.25f, -0.2499f, 0.2499f, 0.25f, 0.2501f, 0.5f}) {
+      for (const float dz : {-32.f, 32.f}) {
+        for (const float tanl : {-2.5f, 2.5f}) {
+          const float qOverPt = halfAngle / (0.5f * o2::constants::math::B2C * bz * dz / tanl);
+          BOOST_TEST_CONTEXT("bz=" << bz << " q/pT=" << qOverPt << " dz=" << dz << " tanl=" << tanl)
+          {
+            auto source = diskState();
+            source.parameters[0] = source.parameters[1] = 0.f;
+            source.parameters[2] = 0.7f;
+            source.parameters[3] = tanl;
+            source.parameters[4] = qOverPt;
+            std::fill(std::begin(source.covariance), std::end(source.covariance), 0.f);
+            source.covariance[packedCovarianceIndex(4, 4)] = 1.f;
+            auto plane = source;
+            plane.referenceCoordinate += dz;
+            std::array<double, 5> parameters{};
+            std::copy(std::begin(source.parameters), std::end(source.parameters), parameters.begin());
+            const auto expected = intersectConversionPlane(source, parameters, plane, bz);
+            constexpr double step = 1.e-3;
+            auto plus = parameters, minus = parameters;
+            plus[4] += step;
+            minus[4] -= step;
+            const auto high = intersectConversionPlane(source, plus, plane, bz);
+            const auto low = intersectConversionPlane(source, minus, plane, bz);
+            std::array<double, 5> derivative{};
+            for (int row = 0; row < 5; ++row) {
+              derivative[row] = (high[row] - low[row]) / (2. * step);
+            }
+
+            auto direct = source;
+            auto referenced = source;
+            SurfaceTrackParameters reference{source};
+            BOOST_REQUIRE(Propagator::propagateForward(direct, plane.referenceCoordinate, bz));
+            BOOST_REQUIRE(Propagator::propagateForward(referenced, reference, plane.referenceCoordinate, bz));
+            for (int row = 0; row < 5; ++row) {
+              // Bound rounding by the operands rather than a possibly cancelling
+              // final angle/component. The covariance/Jacobian tolerance below
+              // remains unchanged from the small-angle regression.
+              const double path = double(dz) / tanl;
+              const double scale = row < 2    ? std::abs(path)
+                                   : row == 2 ? std::abs(parameters[2]) + 2. * std::abs(double(halfAngle))
+                                              : std::abs(expected[row]);
+              const double positionTolerance = 4. * std::numeric_limits<float>::epsilon() * scale + 1.e-12;
+              BOOST_CHECK_SMALL(double(direct.parameters[row]) - expected[row], positionTolerance);
+              BOOST_CHECK_SMALL(double(referenced.parameters[row]) - expected[row], positionTolerance);
+              BOOST_CHECK_SMALL(double(reference.parameters[row]) - expected[row], positionTolerance);
+              for (int column = 0; column <= row; ++column) {
+                const auto index = packedCovarianceIndex(row, column);
+                const double covariance = derivative[row] * derivative[column];
+                const double tolerance = 2.e-5 * std::abs(covariance) + 1.e-16;
+                BOOST_CHECK_SMALL(double(direct.covariance[index]) - covariance, tolerance);
+                BOOST_CHECK_SMALL(double(referenced.covariance[index]) - covariance, tolerance);
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
 BOOST_AUTO_TEST_CASE(ForwardHelixTransportMatchesNumericalDerivatives)
 {
   for (const float bz : {-5.f, 5.f}) {