include/ceres/internal/euler_angles.h

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2022 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#ifndef CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
#define CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_

#include <type_traits>

namespace ceres {
namespace internal {

// The EulerSystem struct represents an Euler Angle Convention in compile time.
// It acts like a trait structure and is also used as a tag for dispatching
// Euler angle conversion function templates
//
// Internally, it implements the convention laid out in "Euler angle
// conversion", Ken Shoemake, Graphics Gems IV, where a choice of axis for the
// first rotation (out of 3) and 3 binary choices compactly specify all 24
// rotation conventions
//
//  - InnerAxis: Axis for the first rotation. This is specified by struct tags
//  axis::X, axis::Y, and axis::Z
//
//  - Parity: Defines the parity of the axis permutation. The axis sequence has
//  Even parity if the second axis of rotation is 'greater-than' the first axis
//  of rotation according to the order X<Y<Z<X, otherwise it has Odd parity.
//  This is specified by struct tags Even and Odd
//
//  - AngleConvention: Defines whether Proper Euler Angles (originally defined
//  by Euler, which has the last axis repeated, i.e. ZYZ, ZXZ, etc), or
//  Tait-Bryan Angles (introduced by the nautical and aerospace fields, i.e.
//  using ZYX for roll-pitch-yaw) are used. This is specified by struct Tags
//  ProperEuler and TaitBryan.
//
//  - FrameConvention: Defines whether the three rotations are be in a global
//  frame of reference (extrinsic) or in a body centred frame of reference
//  (intrinsic). This is specified by struct tags Extrinsic and Intrinsic

namespace axis {
struct X : std::integral_constant<int, 0> {};
struct Y : std::integral_constant<int, 1> {};
struct Z : std::integral_constant<int, 2> {};
}  // namespace axis

struct Even;
struct Odd;

struct ProperEuler;
struct TaitBryan;

struct Extrinsic;
struct Intrinsic;

template <typename InnerAxisType,
          typename ParityType,
          typename AngleConventionType,
          typename FrameConventionType>
struct EulerSystem {
  static constexpr bool kIsParityOdd = std::is_same_v<ParityType, Odd>;
  static constexpr bool kIsProperEuler =
      std::is_same_v<AngleConventionType, ProperEuler>;
  static constexpr bool kIsIntrinsic =
      std::is_same_v<FrameConventionType, Intrinsic>;

  static constexpr int kAxes[3] = {
      InnerAxisType::value,
      (InnerAxisType::value + 1 + static_cast<int>(kIsParityOdd)) % 3,
      (InnerAxisType::value + 2 - static_cast<int>(kIsParityOdd)) % 3};
};

}  // namespace internal

// Define human readable aliases to the type of the tags
using ExtrinsicXYZ = internal::EulerSystem<internal::axis::X,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicXYX = internal::EulerSystem<internal::axis::X,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
using ExtrinsicXZY = internal::EulerSystem<internal::axis::X,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicXZX = internal::EulerSystem<internal::axis::X,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
using ExtrinsicYZX = internal::EulerSystem<internal::axis::Y,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicYZY = internal::EulerSystem<internal::axis::Y,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
using ExtrinsicYXZ = internal::EulerSystem<internal::axis::Y,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicYXY = internal::EulerSystem<internal::axis::Y,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
using ExtrinsicZXY = internal::EulerSystem<internal::axis::Z,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicZXZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
using ExtrinsicZYX = internal::EulerSystem<internal::axis::Z,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Extrinsic>;
using ExtrinsicZYZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Extrinsic>;
/* Rotating axes */
using IntrinsicZYX = internal::EulerSystem<internal::axis::X,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicXYX = internal::EulerSystem<internal::axis::X,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;
using IntrinsicYZX = internal::EulerSystem<internal::axis::X,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicXZX = internal::EulerSystem<internal::axis::X,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;
using IntrinsicXZY = internal::EulerSystem<internal::axis::Y,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicYZY = internal::EulerSystem<internal::axis::Y,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;
using IntrinsicZXY = internal::EulerSystem<internal::axis::Y,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicYXY = internal::EulerSystem<internal::axis::Y,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;
using IntrinsicYXZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Even,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicZXZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Even,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;
using IntrinsicXYZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Odd,
                                           internal::TaitBryan,
                                           internal::Intrinsic>;
using IntrinsicZYZ = internal::EulerSystem<internal::axis::Z,
                                           internal::Odd,
                                           internal::ProperEuler,
                                           internal::Intrinsic>;

}  // namespace ceres

#endif  // CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_