Surfaces/detail/AlignmentHelper.cpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2020 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 http://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/Surfaces/detail/AlignmentHelper.hpp"
0010
0011 #include <algorithm>
0012 #include <cmath>
0013 #include <utility>
0014
0015 Acts::detail::RotationToAxes Acts::detail::rotationToLocalAxesDerivative(
0016     const RotationMatrix3& rotation) {
0017   // Get Euler angles for rotation represented by rotZ * rotY * rotX, i.e.
0018   // first rotation around x axis, then y axis, last z axis
0019   // The elements stored in rotAngles is (rotZ, rotY, rotX)
0020   const Vector3 rotAngles = rotation.eulerAngles(2, 1, 0);
0021   double sx = std::sin(rotAngles(2));
0022   double cx = std::cos(rotAngles(2));
0023   double sy = std::sin(rotAngles(1));
0024   double cy = std::cos(rotAngles(1));
0025   double sz = std::sin(rotAngles(0));
0026   double cz = std::cos(rotAngles(0));
0027   // rotZ * rotY * rotX =
0028   // [ cz*cy  cz*sy*sx-cx*sz  sz*sx+cz*cx*sy ]
0029   // [ cy*sz  cz*cx+sz*sy*sx  cx*sz*sy-cz*sx ]
0030   // [ -sy   cy*sx         cy*cx        ]
0031
0032   // Derivative of local x axis w.r.t. (rotX, rotY, rotZ)
0033   RotationMatrix3 rotToLocalXAxis = RotationMatrix3::Zero();
0034   rotToLocalXAxis.col(0) = Vector3(0, 0, 0);
0035   rotToLocalXAxis.col(1) = Vector3(-cz * sy, -sz * sy, -cy);
0036   rotToLocalXAxis.col(2) = Vector3(-sz * cy, cz * cy, 0);
0037   // Derivative of local y axis w.r.t. (rotX, rotY, rotZ)
0038   RotationMatrix3 rotToLocalYAxis = RotationMatrix3::Zero();
0039   rotToLocalYAxis.col(0) =
0040       Vector3(cz * sy * cx + sz * sx, sz * sy * cx - cz * sx, cy * cx);
0041   rotToLocalYAxis.col(1) = Vector3(cz * cy * sx, sz * cy * sx, -sy * sx);
0042   rotToLocalYAxis.col(2) =
0043       Vector3(-sz * sy * sx - cz * cx, cz * sy * sx - sz * cx, 0);
0044   // Derivative of local z axis w.r.t. (rotX, rotY, rotZ)
0045   RotationMatrix3 rotToLocalZAxis = RotationMatrix3::Zero();
0046   rotToLocalZAxis.col(0) =
0047       Vector3(sz * cx - cz * sy * sx, -sz * sy * sx - cz * cx, -cy * sx);
0048   rotToLocalZAxis.col(1) = Vector3(cz * cy * cx, sz * cy * cx, -sy * cx);
0049   rotToLocalZAxis.col(2) =
0050       Vector3(cz * sx - sz * sy * cx, cz * sy * cx + sz * sx, 0);
0051
0052   return std::make_tuple(std::move(rotToLocalXAxis), std::move(rotToLocalYAxis),
0053                          std::move(rotToLocalZAxis));
0054 }