revert acts-project#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,17 +15,15 @@
 #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] =
+  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
       VectorHelpers::evaluateTrigonomics(direction);
-  ActsScalar invSinTheta = 1. / sinTheta;
   // Prepare the jacobian to curvilinear
   FreeToBoundMatrix freeToCurvJacobian = FreeToBoundMatrix::Zero();
   if (std::abs(cosTheta) < s_curvilinearProjTolerance) {
@@ -63,7 +61,7 @@ FreeToBoundMatrix detail::freeToCurvilinearJacobian(const Vector3& direction) {
 }
 
 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

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

Core/src/Surfaces/Surface.cpp

@@ -12,7 +12,6 @@
 #include "Acts/EventData/detail/TransformationBoundToFree.hpp"
 #include "Acts/Surfaces/SurfaceBounds.hpp"
 #include "Acts/Surfaces/detail/AlignmentHelper.hpp"
-#include "Acts/Utilities/JacobianHelpers.hpp"
 #include "Acts/Utilities/VectorHelpers.hpp"
 
 #include <algorithm>
@@ -262,6 +261,9 @@ Acts::BoundToFreeMatrix Acts::Surface::boundToFreeJacobian(
   const Vector3 position = freeParams.segment<3>(eFreePos0);
   // The direction
   const Vector3 direction = freeParams.segment<3>(eFreeDir0);
+  // Use fast evaluation function of sin/cos
+  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+      VectorHelpers::evaluateTrigonomics(direction);
   // retrieve the reference frame
   const auto rframe = referenceFrame(gctx, position, direction);
   // Initialize the jacobian from local to global
@@ -271,8 +273,11 @@ Acts::BoundToFreeMatrix Acts::Surface::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;
   return jacToGlobal;
 }
@@ -283,6 +288,9 @@ Acts::FreeToBoundMatrix Acts::Surface::freeToBoundJacobian(
   const auto position = parameters.segment<3>(eFreePos0);
   // The direction
   const auto direction = parameters.segment<3>(eFreeDir0);
+  // Use fast evaluation function of sin/cos
+  auto [cosPhi, sinPhi, cosTheta, sinTheta, invSinTheta] =
+      VectorHelpers::evaluateTrigonomics(direction);
   // The measurement frame of the surface
   RotationMatrix3 rframeT =
       referenceFrame(gctx, position, direction).transpose();
@@ -293,8 +301,11 @@ Acts::FreeToBoundMatrix Acts::Surface::freeToBoundJacobian(
   // Time component
   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;
 }