Acts/Utilities/TransformHelpers.hpp

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/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/Tolerance.hpp"
0013
0014 #include <cassert>
0015 #include <stdexcept>
0016
0017 namespace Acts {
0018
0019 /// Check orthogonality within the given tolerance.
0020 /// @param rotation Matrix to check
0021 /// @param tolerance Comparison tolerance (default: s_transformEquivalentTolerance)
0022 /// @return Whether @p rotation is orthogonal
0023 inline bool isOrthogonal(const RotationMatrix3& rotation,
0024                          double tolerance = s_transformEquivalentTolerance) {
0025   return (rotation * rotation.transpose())
0026       .isApprox(RotationMatrix3::Identity(), tolerance);
0027 }
0028
0029 /// Build a rigid transform: `p -> rotation * p + translation`.
0030 /// @pre @p rotation is orthogonal (asserted).
0031 /// @param rotation Local axes in the target frame
0032 /// @param translation Local origin in the target frame
0033 /// @return The rigid transform
0034 inline Transform3 makeTransform3(const RotationMatrix3& rotation,
0035                                  const Vector3& translation = Vector3::Zero()) {
0036   assert(isOrthogonal(rotation) &&
0037          "Transform3 requires an orthogonal rotation part");
0038   Transform3 transform = Transform3::Identity();
0039   transform.linear() = rotation;
0040   transform.translation() = translation;
0041   return transform;
0042 }
0043
0044 /// Convert an affine transform, rejecting a non-orthogonal linear part.
0045 /// @param transform Affine transform to convert
0046 /// @throws std::invalid_argument if the linear part is not orthogonal
0047 /// @return The equivalent rigid transform
0048 inline Transform3 makeTransform3(const AffineTransform3& transform) {
0049   if (!isOrthogonal(transform.linear())) {
0050     throw std::invalid_argument(
0051         "Affine transform is not a rigid transformation");
0052   }
0053   return makeTransform3(transform.linear(), transform.translation());
0054 }
0055
0056 }  // namespace Acts