revert: Use common direction transform Jacobian (#2782)

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, 4> evaluateTrigonomics(const Vector3& direction) {
+inline std::array<ActsScalar, 5> 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::sqrt(1 - z * z);
+  const ActsScalar sinTheta = std::hypot(x, y);
   const ActsScalar invSinTheta = 1. / sinTheta;
   const ActsScalar cosPhi = x * invSinTheta;
   const ActsScalar sinPhi = y * invSinTheta;
 
-  return {cosPhi, sinPhi, cosTheta, sinTheta};
+  return {cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta};
 }
 
 /// Helper method to extract the binning value from a 3D vector.

Core/src/Propagator/detail/CovarianceEngine.cpp

@@ -10,15 +10,13 @@
 
 #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 <optional>

Core/src/Propagator/detail/JacobianEngine.cpp

@@ -15,55 +15,58 @@
 #include "Acts/Geometry/GeometryContext.hpp"
 #include "Acts/Surfaces/Surface.hpp"
 #include "Acts/Utilities/AlgebraHelpers.hpp"
-#include "Acts/Utilities/JacobianHelpers.hpp"
 #include "Acts/Utilities/VectorHelpers.hpp"
 
 #include <cmath>
 
 namespace Acts {
 
 FreeToBoundMatrix detail::freeToCurvilinearJacobian(const Vector3& direction) {
-  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
-      VectorHelpers::evaluateTrigonomics(direction);
-  ActsScalar invSinTheta = 1. / sinTheta;
-  // Prepare the jacobian to curvilinear
-  FreeToBoundMatrix freeToCurvJacobian = FreeToBoundMatrix::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;
+  // prepare the jacobian to curvilinear
+  FreeToBoundMatrix jacToCurv = FreeToBoundMatrix::Zero();
   if (std::abs(cosTheta) < s_curvilinearProjTolerance) {
     // We normally operate in curvilinear coordinates defined as follows
-    freeToCurvJacobian(eBoundLoc0, eFreePos0) = -sinPhi;
-    freeToCurvJacobian(eBoundLoc0, eFreePos1) = cosPhi;
-    freeToCurvJacobian(eBoundLoc1, eFreePos0) = -cosPhi * cosTheta;
-    freeToCurvJacobian(eBoundLoc1, eFreePos1) = -sinPhi * cosTheta;
-    freeToCurvJacobian(eBoundLoc1, eFreePos2) = sinTheta;
+    jacToCurv(0, 0) = -sinPhi;
+    jacToCurv(0, 1) = cosPhi;
+    jacToCurv(1, 0) = -cosPhi * cosTheta;
+    jacToCurv(1, 1) = -sinPhi * cosTheta;
+    jacToCurv(1, 2) = sinTheta;
   } else {
     // Under grazing incidence to z, the above coordinate system definition
     // becomes numerically unstable, and we need to switch to another one
-    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)
-    const ActsScalar c = std::hypot(y, z);
-    const ActsScalar invC = 1. / c;
-    freeToCurvJacobian(eBoundLoc0, eFreePos1) = -z * invC;
-    freeToCurvJacobian(eBoundLoc0, eFreePos2) = y * invC;
-    freeToCurvJacobian(eBoundLoc1, eFreePos0) = c;
-    freeToCurvJacobian(eBoundLoc1, eFreePos1) = -x * y * invC;
-    freeToCurvJacobian(eBoundLoc1, eFreePos2) = -x * z * invC;
+    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;
   }
   // Time parameter
-  freeToCurvJacobian(eBoundTime, eFreeTime) = 1.;
+  jacToCurv(5, 3) = 1.;
   // Directional and momentum parameters for curvilinear
-  freeToCurvJacobian(eBoundPhi, eFreeDir0) = -sinPhi * invSinTheta;
-  freeToCurvJacobian(eBoundPhi, eFreeDir1) = cosPhi * invSinTheta;
-  freeToCurvJacobian(eBoundTheta, eFreeDir0) = cosPhi * cosTheta;
-  freeToCurvJacobian(eBoundTheta, eFreeDir1) = sinPhi * cosTheta;
-  freeToCurvJacobian(eBoundTheta, eFreeDir2) = -sinTheta;
-  freeToCurvJacobian(eBoundQOverP, eFreeQOverP) = 1.;
-
-  return freeToCurvJacobian;
+  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.;
+
+  return jacToCurv;
 }
 
 BoundToFreeMatrix detail::curvilinearToFreeJacobian(const Vector3& direction) {
-  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
 
   // Prepare the jacobian to free
@@ -219,17 +222,28 @@ void detail::reinitializeJacobians(FreeMatrix& freeTransportJacobian,
   freeToPathDerivatives = FreeVector::Zero();
   boundToFreeJacobian = BoundToFreeMatrix::Zero();
 
-  auto [cosPhi, sinPhi, cosTheta, sinTheta] =
-      VectorHelpers::evaluateTrigonomics(direction);
+  // 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;
 
   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.block<3, 2>(eFreeDir0, eBoundPhi) =
-      sphericalToFreeDirectionJacobian(direction);
+  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(eFreeQOverP, eBoundQOverP) = 1;
 }
 

Core/src/Surfaces/DiscSurface.cpp

@@ -21,7 +21,6 @@
 #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>
@@ -219,26 +218,34 @@ 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 lcphi = std::cos(lphi);
-  double lsphi = std::sin(lphi);
+  double lcos_phi = cos(lphi);
+  double lsin_phi = 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) =
-      lcphi * rframe.block<3, 1>(0, 0) + lsphi * rframe.block<3, 1>(0, 1);
+      lcos_phi * rframe.block<3, 1>(0, 0) + lsin_phi * rframe.block<3, 1>(0, 1);
   jacToGlobal.block<3, 1>(eFreePos0, eBoundLoc1) =
-      lrad *
-      (lcphi * rframe.block<3, 1>(0, 1) - lsphi * rframe.block<3, 1>(0, 0));
+      lrad * (lcos_phi * rframe.block<3, 1>(0, 1) -
+              lsin_phi * rframe.block<3, 1>(0, 0));
   // the time component
   jacToGlobal(eFreeTime, eBoundTime) = 1;
   // the momentum components
-  jacToGlobal.block<3, 2>(eFreeDir0, eBoundPhi) =
-      sphericalToFreeDirectionJacobian(direction);
+  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(eFreeQOverP, eBoundQOverP) = 1;
   return jacToGlobal;
 }
@@ -251,15 +258,25 @@ 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 = std::cos(lphi);
-  const double lsphi = std::sin(lphi);
+  const double lcphi = cos(lphi);
+  const double lsphi = sin(lphi);
   // rotate into the polar coorindates
   auto lx = rframeT.block<1, 3>(0, 0);
   auto ly = rframeT.block<1, 3>(1, 0);
@@ -272,8 +289,11 @@ Acts::FreeToBoundMatrix Acts::DiscSurface::freeToBoundJacobian(
   // Time element
   jacToLocal(eBoundTime, eFreeTime) = 1;
   // Directional and momentum elements for reference frame surface
-  jacToLocal.block<2, 3>(eBoundPhi, eFreeDir0) =
-      freeToSphericalDirectionJacobian(direction);
+  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(eBoundQOverP, eFreeQOverP) = 1;
   return jacToLocal;
 }

Core/src/Surfaces/LineSurface.cpp

@@ -18,7 +18,6 @@
 #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>
@@ -205,6 +204,11 @@ 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);
 
@@ -216,8 +220,11 @@ Acts::BoundToFreeMatrix Acts::LineSurface::boundToFreeJacobian(
   // the time component
   jacToGlobal(eFreeTime, eBoundTime) = 1;
   // the momentum components
-  jacToGlobal.block<3, 2>(eFreeDir0, eBoundPhi) =
-      sphericalToFreeDirectionJacobian(direction);
+  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(eFreeQOverP, eBoundQOverP) = 1;
 
   // For the derivative of global position with bound angles, refer the