src/Utilities/TransformComparator.cpp

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 #include "Acts/Utilities/detail/TransformComparator.hpp"
0010
0011 #include "Acts/Utilities/MathHelpers.hpp"
0012 #include "Acts/Utilities/detail/EigenCompat.hpp"
0013
0014 namespace Acts::detail {
0015 TransformComparator::TransformComparator(const double transTolerance,
0016                                          const double rotTolerance)
0017     : m_tolTrans{transTolerance}, m_tolRot{rotTolerance} {}
0018 int TransformComparator::compare(const Acts::RotationMatrix3& a,
0019                                  const Acts::RotationMatrix3& b) const {
0020   const Acts::Vector3 anglesA =
0021       detail::EigenCompat::canonicalEulerAngles(a, 2, 1, 0);
0022   const Acts::Vector3 anglesB =
0023       detail::EigenCompat::canonicalEulerAngles(b, 2, 1, 0);
0024   for (int i = 0; i < 3; ++i) {
0025     const double diff = anglesA[i] - anglesB[i];
0026     if (Acts::abs(diff) > m_tolRot) {
0027       return copySign(1, diff);
0028     }
0029   }
0030   return 0;
0031 }
0032 int TransformComparator::compare(const Acts::Transform3& a,
0033                                  const Acts::Transform3& b) const {
0034   if (const int tCmp = compare<3>(a.translation(), b.translation());
0035       tCmp != 0) {
0036     return tCmp;
0037   }
0038   return compare(a.rotation(), b.rotation());
0039 }
0040 bool TransformComparator::operator()(const Acts::Transform3& a,
0041                                      const Acts::Transform3& b) const {
0042   return compare(a, b) < 0;
0043 }
0044 bool TransformComparator::operator()(const Acts::RotationMatrix3& a,
0045                                      const RotationMatrix3& b) const {
0046   return compare(a, b) < 0;
0047 }
0048 bool TransformComparator::operator()(const Acts::Vector3& a,
0049                                      const Acts::Vector3& b) const {
0050   return compare<3>(a, b) < 0;
0051 }
0052 bool TransformComparator::operator()(const Acts::Vector2& a,
0053                                      const Acts::Vector2& b) const {
0054   return compare<2>(a, b) < 0;
0055 }
0056 }  // namespace Acts::detail