refactor: Use common direction transform Jacobian (acts-project#2782)

I found the same code in a few places and there seems to be a bit of simplification opportunity in the math

Core/include/Acts/Utilities/JacobianHelpers.hpp

@@ -0,0 +1,68 @@
+// This file is part of the Acts project.
+//
+// Copyright (C) 2023 CERN for the benefit of the Acts project
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include "Acts/Definitions/Algebra.hpp"
+#include "Acts/Utilities/VectorHelpers.hpp"
+
+namespace Acts {
+
+/// @brief Calculates the Jacobian for spherical to free
+///        direction vector transformation
+///
+/// @note We use the direction vector as an input because
+///       the trigonometric simplify that way
+///
+/// @param direction The normalised direction vector
+///
+/// @return The Jacobian d(dir_x, dir_y, dir_z) / d(phi, theta)
+///
+inline ActsMatrix<3, 2> sphericalToFreeDirectionJacobian(
+    const Vector3& direction) {
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
+      VectorHelpers::evaluateTrigonomics(direction);
+
+  // clang-format off
+  ActsMatrix<3, 2> jacobian;
+  jacobian << 
+    -direction.y(),  cosTheta * cosPhi,
+     direction.x(),  cosTheta * sinPhi,
+     0,             -sinTheta;
+  // clang-format on
+
+  return jacobian;
+}
+
+/// @brief Calculates the Jacobian for free to spherical
+///        direction vector transformation
+///
+/// @note We use the direction vector as an input because
+///       the trigonometric simplify that way
+///
+/// @param direction The normalised direction vector
+///
+/// @return The Jacobian d(phi, theta) / d(dir_x, dir_y, dir_z)
+///
+inline ActsMatrix<2, 3> freeToSphericalDirectionJacobian(
+    const Vector3& direction) {
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
+      VectorHelpers::evaluateTrigonomics(direction);
+  ActsScalar invSinTheta = 1. / sinTheta;
+
+  // clang-format off
+  ActsMatrix<2, 3> jacobian;
+  jacobian <<
+    -sinPhi * invSinTheta, cosPhi * invSinTheta, 0,
+     cosPhi * cosTheta,    sinPhi * cosTheta,    -sinTheta;
+  // clang-format on
+
+  return jacobian;
+}
+
+}  // namespace Acts

Core/include/Acts/Utilities/VectorHelpers.hpp

@@ -132,18 +132,18 @@ double eta(const Eigen::MatrixBase<Derived>& v) noexcept {
 /// @param direction for this evaluatoin
 ///
 /// @return cos(phi), sin(phi), cos(theta), sin(theta), 1/sin(theta)
-inline std::array<ActsScalar, 5> evaluateTrigonomics(const Vector3& direction) {
+inline std::array<ActsScalar, 4> evaluateTrigonomics(const Vector3& direction) {
   const ActsScalar x = direction(0);  // == cos(phi) * sin(theta)
   const ActsScalar y = direction(1);  // == sin(phi) * sin(theta)
   const ActsScalar z = direction(2);  // == cos(theta)
   // can be turned into cosine/sine
   const ActsScalar cosTheta = z;
-  const ActsScalar sinTheta = std::hypot(x, y);
+  const ActsScalar sinTheta = std::sqrt(1 - z * z);
   const ActsScalar invSinTheta = 1. / sinTheta;
   const ActsScalar cosPhi = x * invSinTheta;
   const ActsScalar sinPhi = y * invSinTheta;
 
-  return {cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta};
+  return {cosPhi, sinPhi, cosTheta, sinTheta};
 }
 
 /// Helper method to extract the binning value from a 3D vector.

Core/src/Propagator/detail/CovarianceEngine.cpp

@@ -10,13 +10,15 @@
 
 #include "Acts/Definitions/Common.hpp"
 #include "Acts/Definitions/Tolerance.hpp"
+#include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/EventData/GenericBoundTrackParameters.hpp"
 #include "Acts/EventData/GenericCurvilinearTrackParameters.hpp"
 #include "Acts/EventData/detail/CorrectedTransformationFreeToBound.hpp"
 #include "Acts/EventData/detail/TransformationBoundToFree.hpp"
 #include "Acts/EventData/detail/TransformationFreeToBound.hpp"
 #include "Acts/Propagator/detail/JacobianEngine.hpp"
 #include "Acts/Utilities/AlgebraHelpers.hpp"
+#include "Acts/Utilities/JacobianHelpers.hpp"
 #include "Acts/Utilities/Result.hpp"
 
 #include <algorithm>
@@ -36,6 +38,7 @@ using CurvilinearState =
     std::tuple<CurvilinearTrackParameters, Jacobian, double>;
 
 /// @brief Evaluate the projection Jacobian from free to curvilinear parameters
+/// without transport jacobian.
 ///
 /// @param [in] direction Normalised direction vector
 ///
@@ -45,41 +48,36 @@ FreeToBoundMatrix freeToCurvilinearJacobian(const Vector3& direction) {
   const double x = direction(0);  // == cos(phi) * sin(theta)
   const double y = direction(1);  // == sin(phi) * sin(theta)
   const double z = direction(2);  // == cos(theta)
-  // can be turned into cosine/sine
-  const double cosTheta = z;
-  const double sinTheta = std::hypot(x, y);
-  const double invSinTheta = 1. / sinTheta;
-  const double cosPhi = x * invSinTheta;
-  const double sinPhi = y * invSinTheta;
   // prepare the jacobian to curvilinear
   FreeToBoundMatrix jacToCurv = FreeToBoundMatrix::Zero();
-  if (std::abs(cosTheta) < s_curvilinearProjTolerance) {
+  if (std::abs(z) < s_curvilinearProjTolerance) {
+    auto [cosPhi, sinPhi, cosTheta, sinTheta] =
+        VectorHelpers::evaluateTrigonomics(direction);
     // We normally operate in curvilinear coordinates defined as follows
-    jacToCurv(0, 0) = -sinPhi;
-    jacToCurv(0, 1) = cosPhi;
-    jacToCurv(1, 0) = -cosPhi * cosTheta;
-    jacToCurv(1, 1) = -sinPhi * cosTheta;
-    jacToCurv(1, 2) = sinTheta;
+    jacToCurv(eBoundLoc0, eFreePos0) = -sinPhi;
+    jacToCurv(eBoundLoc0, eFreePos1) = cosPhi;
+    // jacToCurv(eBoundLoc0, eFreePos2) = 0;
+    jacToCurv(eBoundLoc1, eFreePos0) = -cosPhi * cosTheta;
+    jacToCurv(eBoundLoc1, eFreePos1) = -sinPhi * cosTheta;
+    jacToCurv(eBoundLoc1, eFreePos2) = sinTheta;
   } else {
     // Under grazing incidence to z, the above coordinate system definition
     // becomes numerically unstable, and we need to switch to another one
     const double c = std::hypot(y, z);
     const double invC = 1. / c;
-    jacToCurv(0, 1) = -z * invC;
-    jacToCurv(0, 2) = y * invC;
-    jacToCurv(1, 0) = c;
-    jacToCurv(1, 1) = -x * y * invC;
-    jacToCurv(1, 2) = -x * z * invC;
+    // jacToCurv(eBoundLoc0, eFreePos0) = 0;
+    jacToCurv(eBoundLoc0, eFreePos1) = -z * invC;
+    jacToCurv(eBoundLoc0, eFreePos2) = y * invC;
+    jacToCurv(eBoundLoc1, eFreePos0) = c;
+    jacToCurv(eBoundLoc1, eFreePos1) = -x * y * invC;
+    jacToCurv(eBoundLoc1, eFreePos2) = -x * z * invC;
   }
   // Time parameter
-  jacToCurv(5, 3) = 1.;
+  jacToCurv(eBoundTime, eFreeTime) = 1.;
   // Directional and momentum parameters for curvilinear
-  jacToCurv(2, 4) = -sinPhi * invSinTheta;
-  jacToCurv(2, 5) = cosPhi * invSinTheta;
-  jacToCurv(3, 4) = cosPhi * cosTheta;
-  jacToCurv(3, 5) = sinPhi * cosTheta;
-  jacToCurv(3, 6) = -sinTheta;
-  jacToCurv(4, 7) = 1.;
+  jacToCurv.block<2, 3>(eBoundPhi, eFreeDir0) =
+      freeToSphericalDirectionJacobian(direction);
+  jacToCurv(eBoundQOverP, eFreeQOverP) = 1.;
 
   return jacToCurv;
 }
@@ -237,28 +235,17 @@ void reinitializeJacobians(FreeMatrix& freeTransportJacobian,
   freeToPathDerivatives = FreeVector::Zero();
   boundToFreeJacobian = BoundToFreeMatrix::Zero();
 
-  // Optimized trigonometry on the propagation direction
-  const double x = direction(0);  // == cos(phi) * sin(theta)
-  const double y = direction(1);  // == sin(phi) * sin(theta)
-  const double z = direction(2);  // == cos(theta)
-  // can be turned into cosine/sine
-  const double cosTheta = z;
-  const double sinTheta = std::hypot(x, y);
-  const double invSinTheta = 1. / sinTheta;
-  const double cosPhi = x * invSinTheta;
-  const double sinPhi = y * invSinTheta;
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
+      VectorHelpers::evaluateTrigonomics(direction);
 
   boundToFreeJacobian(eFreePos0, eBoundLoc0) = -sinPhi;
   boundToFreeJacobian(eFreePos0, eBoundLoc1) = -cosPhi * cosTheta;
   boundToFreeJacobian(eFreePos1, eBoundLoc0) = cosPhi;
   boundToFreeJacobian(eFreePos1, eBoundLoc1) = -sinPhi * cosTheta;
   boundToFreeJacobian(eFreePos2, eBoundLoc1) = sinTheta;
   boundToFreeJacobian(eFreeTime, eBoundTime) = 1;
-  boundToFreeJacobian(eFreeDir0, eBoundPhi) = -sinTheta * sinPhi;
-  boundToFreeJacobian(eFreeDir0, eBoundTheta) = cosTheta * cosPhi;
-  boundToFreeJacobian(eFreeDir1, eBoundPhi) = sinTheta * cosPhi;
-  boundToFreeJacobian(eFreeDir1, eBoundTheta) = cosTheta * sinPhi;
-  boundToFreeJacobian(eFreeDir2, eBoundTheta) = -sinTheta;
+  boundToFreeJacobian.block<3, 2>(eFreeDir0, eBoundPhi) =
+      sphericalToFreeDirectionJacobian(direction);
   boundToFreeJacobian(eFreeQOverP, eBoundQOverP) = 1;
 }
 }  // namespace

Core/src/Propagator/detail/JacobianEngine.cpp

@@ -22,8 +22,9 @@ namespace Acts {
 namespace detail {
 
 FreeToBoundMatrix freeToCurvilinearJacobian(const Vector3& direction) {
-  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
+  ActsScalar invSinTheta = 1. / sinTheta;
   // Prepare the jacobian to curvilinear
   FreeToBoundMatrix freeToCurvJacobian = FreeToBoundMatrix::Zero();
   if (std::abs(cosTheta) < s_curvilinearProjTolerance) {
@@ -61,7 +62,7 @@ FreeToBoundMatrix freeToCurvilinearJacobian(const Vector3& direction) {
 }
 
 BoundToFreeMatrix curvilinearToFreeJacobian(const Vector3& direction) {
-  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
 
   // Prepare the jacobian to free
@@ -88,7 +89,7 @@ BoundToFreeMatrix curvilinearToFreeJacobian(const Vector3& direction) {
 ActsMatrix<8, 7> anglesToDirectionJacobian(const Vector3& direction) {
   ActsMatrix<8, 7> jacobian = ActsMatrix<8, 7>::Zero();
 
-  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
 
   jacobian(0, 0) = 1.;
@@ -108,8 +109,9 @@ ActsMatrix<8, 7> anglesToDirectionJacobian(const Vector3& direction) {
 ActsMatrix<7, 8> directionToAnglesJacobian(const Vector3& direction) {
   ActsMatrix<7, 8> jacobian = ActsMatrix<7, 8>::Zero();
 
-  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
+  ActsScalar invSinTheta = 1. / sinTheta;
 
   jacobian(0, 0) = 1.;
   jacobian(1, 1) = 1.;

Core/src/Surfaces/DiscSurface.cpp

@@ -21,6 +21,7 @@
 #include "Acts/Surfaces/detail/PlanarHelper.hpp"
 #include "Acts/Utilities/Helpers.hpp"
 #include "Acts/Utilities/Intersection.hpp"
+#include "Acts/Utilities/JacobianHelpers.hpp"
 #include "Acts/Utilities/ThrowAssert.hpp"
 
 #include <algorithm>
@@ -218,34 +219,26 @@ Acts::BoundToFreeMatrix Acts::DiscSurface::boundToFreeJacobian(
   const Vector3 position = freeParams.segment<3>(eFreePos0);
   // The direction
   const Vector3 direction = freeParams.segment<3>(eFreeDir0);
-  // Get the sines and cosines directly
-  const double cos_theta = std::cos(boundParams[eBoundTheta]);
-  const double sin_theta = std::sin(boundParams[eBoundTheta]);
-  const double cos_phi = std::cos(boundParams[eBoundPhi]);
-  const double sin_phi = std::sin(boundParams[eBoundPhi]);
   // special polar coordinates for the Disc
   double lrad = boundParams[eBoundLoc0];
   double lphi = boundParams[eBoundLoc1];
-  double lcos_phi = cos(lphi);
-  double lsin_phi = sin(lphi);
+  double lcphi = std::cos(lphi);
+  double lsphi = std::sin(lphi);
   // retrieve the reference frame
   const auto rframe = referenceFrame(gctx, position, direction);
   // Initialize the jacobian from local to global
   BoundToFreeMatrix jacToGlobal = BoundToFreeMatrix::Zero();
   // the local error components - rotated from reference frame
   jacToGlobal.block<3, 1>(eFreePos0, eBoundLoc0) =
-      lcos_phi * rframe.block<3, 1>(0, 0) + lsin_phi * rframe.block<3, 1>(0, 1);
+      lcphi * rframe.block<3, 1>(0, 0) + lsphi * rframe.block<3, 1>(0, 1);
   jacToGlobal.block<3, 1>(eFreePos0, eBoundLoc1) =
-      lrad * (lcos_phi * rframe.block<3, 1>(0, 1) -
-              lsin_phi * rframe.block<3, 1>(0, 0));
+      lrad *
+      (lcphi * rframe.block<3, 1>(0, 1) - lsphi * rframe.block<3, 1>(0, 0));
   // the time component
   jacToGlobal(eFreeTime, eBoundTime) = 1;
   // the momentum components
-  jacToGlobal(eFreeDir0, eBoundPhi) = (-sin_theta) * sin_phi;
-  jacToGlobal(eFreeDir0, eBoundTheta) = cos_theta * cos_phi;
-  jacToGlobal(eFreeDir1, eBoundPhi) = sin_theta * cos_phi;
-  jacToGlobal(eFreeDir1, eBoundTheta) = cos_theta * sin_phi;
-  jacToGlobal(eFreeDir2, eBoundTheta) = (-sin_theta);
+  jacToGlobal.block<3, 2>(eFreeDir0, eBoundPhi) =
+      sphericalToFreeDirectionJacobian(direction);
   jacToGlobal(eFreeQOverP, eBoundQOverP) = 1;
   return jacToGlobal;
 }
@@ -258,25 +251,15 @@ Acts::FreeToBoundMatrix Acts::DiscSurface::freeToBoundJacobian(
   const auto position = parameters.segment<3>(eFreePos0);
   // The direction
   const auto direction = parameters.segment<3>(eFreeDir0);
-  // Optimized trigonometry on the propagation direction
-  const double x = direction(0);  // == cos(phi) * sin(theta)
-  const double y = direction(1);  // == sin(phi) * sin(theta)
-  const double z = direction(2);  // == cos(theta)
-  // can be turned into cosine/sine
-  const double cosTheta = z;
-  const double sinTheta = std::hypot(x, y);
-  const double invSinTheta = 1. / sinTheta;
-  const double cosPhi = x * invSinTheta;
-  const double sinPhi = y * invSinTheta;
   // The measurement frame of the surface
   RotationMatrix3 rframeT =
       referenceFrame(gctx, position, direction).transpose();
   // calculate the transformation to local coordinates
   const Vector3 pos_loc = transform(gctx).inverse() * position;
   const double lr = perp(pos_loc);
   const double lphi = phi(pos_loc);
-  const double lcphi = cos(lphi);
-  const double lsphi = sin(lphi);
+  const double lcphi = std::cos(lphi);
+  const double lsphi = std::sin(lphi);
   // rotate into the polar coorindates
   auto lx = rframeT.block<1, 3>(0, 0);
   auto ly = rframeT.block<1, 3>(1, 0);
@@ -289,11 +272,8 @@ Acts::FreeToBoundMatrix Acts::DiscSurface::freeToBoundJacobian(
   // Time element
   jacToLocal(eBoundTime, eFreeTime) = 1;
   // Directional and momentum elements for reference frame surface
-  jacToLocal(eBoundPhi, eFreeDir0) = -sinPhi * invSinTheta;
-  jacToLocal(eBoundPhi, eFreeDir1) = cosPhi * invSinTheta;
-  jacToLocal(eBoundTheta, eFreeDir0) = cosPhi * cosTheta;
-  jacToLocal(eBoundTheta, eFreeDir1) = sinPhi * cosTheta;
-  jacToLocal(eBoundTheta, eFreeDir2) = -sinTheta;
+  jacToLocal.block<2, 3>(eBoundPhi, eFreeDir0) =
+      freeToSphericalDirectionJacobian(direction);
   jacToLocal(eBoundQOverP, eFreeQOverP) = 1;
   return jacToLocal;
 }

Core/src/Surfaces/LineSurface.cpp

@@ -18,6 +18,7 @@
 #include "Acts/Surfaces/detail/AlignmentHelper.hpp"
 #include "Acts/Utilities/Helpers.hpp"
 #include "Acts/Utilities/Intersection.hpp"
+#include "Acts/Utilities/JacobianHelpers.hpp"
 #include "Acts/Utilities/ThrowAssert.hpp"
 
 #include <algorithm>
@@ -204,11 +205,6 @@ Acts::BoundToFreeMatrix Acts::LineSurface::boundToFreeJacobian(
   Vector3 position = freeParams.segment<3>(eFreePos0);
   // The direction
   Vector3 direction = freeParams.segment<3>(eFreeDir0);
-  // Get the sines and cosines directly
-  double cosTheta = std::cos(boundParams[eBoundTheta]);
-  double sinTheta = std::sin(boundParams[eBoundTheta]);
-  double cosPhi = std::cos(boundParams[eBoundPhi]);
-  double sinPhi = std::sin(boundParams[eBoundPhi]);
   // retrieve the reference frame
   auto rframe = referenceFrame(gctx, position, direction);
 
@@ -220,11 +216,8 @@ Acts::BoundToFreeMatrix Acts::LineSurface::boundToFreeJacobian(
   // the time component
   jacToGlobal(eFreeTime, eBoundTime) = 1;
   // the momentum components
-  jacToGlobal(eFreeDir0, eBoundPhi) = -sinTheta * sinPhi;
-  jacToGlobal(eFreeDir0, eBoundTheta) = cosTheta * cosPhi;
-  jacToGlobal(eFreeDir1, eBoundPhi) = sinTheta * cosPhi;
-  jacToGlobal(eFreeDir1, eBoundTheta) = cosTheta * sinPhi;
-  jacToGlobal(eFreeDir2, eBoundTheta) = -sinTheta;
+  jacToGlobal.block<3, 2>(eFreeDir0, eBoundPhi) =
+      sphericalToFreeDirectionJacobian(direction);
   jacToGlobal(eFreeQOverP, eBoundQOverP) = 1;
 
   // For the derivative of global position with bound angles, refer the