Utilities/detail/Line3DWithPartialDerivatives.ipp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/Utilities/detail/Line3DWithPartialDerivatives.hpp"
0012
0013 #include "Acts/Utilities/Helpers.hpp"
0014 #include "Acts/Utilities/UnitVectors.hpp"
0015 #include "Acts/Utilities/VectorHelpers.hpp"
0016
0017 namespace Acts::detail {
0018
0019 template <std::floating_point T>
0020 Line3DWithPartialDerivatives<T>::ParamVector
0021 Line3DWithPartialDerivatives<T>::parameters() const {
0022   ParamVector pars{};
0023   pars[toUnderlying(ParIndex::x0)] = m_pos[Acts::eX];
0024   pars[toUnderlying(ParIndex::y0)] = m_pos[Acts::eY];
0025   pars[toUnderlying(ParIndex::theta)] = Acts::VectorHelpers::theta(m_dir);
0026   pars[toUnderlying(ParIndex::phi)] = Acts::VectorHelpers::phi(m_dir);
0027   return pars;
0028 }
0029
0030 template <std::floating_point T>
0031 template <std::size_t N>
0032 Line3DWithPartialDerivatives<T>::Line3DWithPartialDerivatives(
0033     const std::array<T, N>& initPars) noexcept {
0034   updateParameters(initPars);
0035 }
0036
0037 template <std::floating_point T>
0038 template <std::size_t N>
0039 void Line3DWithPartialDerivatives<T>::updateParameters(
0040     const std::array<T, N>& newPars) noexcept
0041   requires(N >= s_nPars)
0042 {
0043   m_pos[Acts::eX] = newPars[toUnderlying(ParIndex::x0)];
0044   m_pos[Acts::eY] = newPars[toUnderlying(ParIndex::y0)];
0045
0046   const T cosTheta = std::cos(newPars[toUnderlying(ParIndex::theta)]);
0047   const T sinTheta = std::sin(newPars[toUnderlying(ParIndex::theta)]);
0048   const T cosPhi = std::cos(newPars[toUnderlying(ParIndex::phi)]);
0049   const T sinPhi = std::sin(newPars[toUnderlying(ParIndex::phi)]);
0050
0051   m_dir = Vector{cosPhi * sinTheta, sinPhi * sinTheta, cosTheta};
0052
0053   m_gradient[toUnderlying(ParIndex::y0)] = Vector::UnitY();
0054   m_gradient[toUnderlying(ParIndex::x0)] = Vector::UnitX();
0055   /**            x_{0}                cos (phi) sin (theta)
0056    *  position = y_{0}  , Direction = sin (phi) sin (theta)
0057    *             0                         cos theta
0058    *
0059    *
0060    *   d Direction     cos (phi) cos(theta)     d Direction     -sin(phi) sin
0061    *(theta)
0062    *  -------------=   sin (phi) cos(theta)     ------------ =   cos(phi) sin
0063    *(theta) dTheta         - sin (theta)             dPhi                 0
0064    *
0065    *******************************************************************************/
0066   m_gradient[toUnderlying(ParIndex::theta)] =
0067       Vector{cosPhi * cosTheta, sinPhi * cosTheta, -sinTheta};
0068   m_gradient[toUnderlying(ParIndex::phi)] =
0069       Vector{-sinTheta * sinPhi, sinTheta * cosPhi, 0};
0070   /*********************************************************************************
0071    *   Non-vanishing second order derivatives
0072    *
0073    *    d^{2} Direction                 d^{2} Direction                     cos
0074    *phi
0075    *    ------------- = - Direction ,   ------------      = - sin(theta)    sin
0076    *phi d^{2} theta                      d^{2} phi                             0
0077    *
0078    *   d^{2} Direction                 -sin phi
0079    *   -------------     = cos(theta)   cos phi
0080    *    d theta dPhi                       0
0081    ************************************************************************************/
0082   constexpr auto idxThetaSq = vecIdxFromSymMat<s_nPars>(
0083       toUnderlying(ParIndex::theta), toUnderlying(ParIndex::theta));
0084   constexpr auto idxPhiSq = vecIdxFromSymMat<s_nPars>(
0085       toUnderlying(ParIndex::phi), toUnderlying(ParIndex::phi));
0086   constexpr auto idxPhiTheta = vecIdxFromSymMat<s_nPars>(
0087       toUnderlying(ParIndex::theta), toUnderlying(ParIndex::phi));
0088   m_hessian[idxThetaSq] = -m_dir;
0089   m_hessian[idxPhiSq] = -sinTheta * Vector{cosPhi, sinPhi, 0.};
0090   m_hessian[idxPhiTheta] = cosTheta * Vector{-sinPhi, cosPhi, 0.};
0091 }
0092
0093 template <std::floating_point T>
0094 const Line3DWithPartialDerivatives<T>::Vector&
0095 Line3DWithPartialDerivatives<T>::position() const {
0096   return m_pos;
0097 }
0098 template <std::floating_point T>
0099 const Line3DWithPartialDerivatives<T>::Vector&
0100 Line3DWithPartialDerivatives<T>::direction() const {
0101   return m_dir;
0102 }
0103 template <std::floating_point T>
0104 const Line3DWithPartialDerivatives<T>::Vector&
0105 Line3DWithPartialDerivatives<T>::gradient(const ParIndex par) const {
0106   const auto param = static_cast<std::uint8_t>(par);
0107   assert(param < m_gradient.size());
0108   return m_gradient[param];
0109 }
0110 template <std::floating_point T>
0111 const Line3DWithPartialDerivatives<T>::Vector&
0112 Line3DWithPartialDerivatives<T>::hessian(const ParIndex param1,
0113                                          const ParIndex param2) const {
0114   const auto idx{vecIdxFromSymMat<s_nPars>(static_cast<std::size_t>(param1),
0115                                            static_cast<std::size_t>(param2))};
0116   assert(idx < m_hessian.size());
0117   return m_hessian[idx];
0118 }
0119
0120 template <std::floating_point T>
0121 Line3DWithPartialDerivatives<T>::Vector Line3DWithPartialDerivatives<T>::point(
0122     const double lambda) const {
0123   return position() + lambda * direction();
0124 }
0125 }  // namespace Acts::detail