RoboidControl-cpp/LinearAlgebra/SwingTwist.cpp

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.

#include "SwingTwist.h"

template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
  this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
  this->twist = AngleOf<T>();
}

template <typename T>
SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist) {
  // Normalize angles
  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);

  if (swing.vertical > deg90 || swing.vertical < -deg90) {
    swing.horizontal += deg180;
    swing.vertical = deg180 - swing.vertical;
    twist += deg180;
  }

  this->swing = swing;
  this->twist = twist;
}

template <typename T>
SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
                              AngleOf<T> vertical,
                              AngleOf<T> twist) {
  // Normalize angles
  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);

  if (vertical > deg90 || vertical < -deg90) {
    horizontal += deg180;
    vertical = deg180 - vertical;
    twist += deg180;
  }

  this->swing = DirectionOf<T>(horizontal, vertical);
  this->twist = twist;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
                                         float vertical,
                                         float twist) {
  SwingTwistOf<T> orientation = SwingTwistOf<T>(AngleOf<T>::Degrees(horizontal),
                                                -AngleOf<T>::Degrees(vertical),
                                                AngleOf<T>::Degrees(twist));
  // DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
  // AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
  // SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
  return orientation;
}

template <typename T>
Quaternion SwingTwistOf<T>::ToQuaternion() const {
  Quaternion q = Quaternion::Euler(-this->swing.vertical.InDegrees(),
                                   this->swing.horizontal.InDegrees(),
                                   this->twist.InDegrees());
  return q;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::FromQuaternion(Quaternion q) {
  Vector3 angles = Quaternion::ToAngles(q);
  SwingTwistOf<T> r =
      SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());
  r.Normalize();
  return r;
}

template <typename T>
SphericalOf<T> SwingTwistOf<T>::ToAngleAxis() const {
  Quaternion q = this->ToQuaternion();
  float angle;
  Vector3 axis;
  q.ToAngleAxis(&angle, &axis);
  DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);

  SphericalOf<T> aa = SphericalOf<T>(angle, direction);
  return aa;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::FromAngleAxis(SphericalOf<T> aa) {
  Vector3 vectorAxis = aa.direction.ToVector3();
  Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
  return SwingTwistOf<T>();
}

template <typename T>
bool SwingTwistOf<T>::operator==(const SwingTwistOf<T> s) const {
  return (this->swing == s.swing) && (this->twist == s.twist);
}

template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();

template <typename T>
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
  SphericalOf<T> v = SphericalOf<T>(
      vector.distance, vector.direction.horizontal + this->swing.horizontal,
      vector.direction.vertical + this->swing.vertical);
  return v;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*(
    const SwingTwistOf<T>& rotation) const {
  SwingTwistOf<T> r =
      SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
                   this->swing.vertical + rotation.swing.vertical,
                   this->twist + rotation.twist);
  return r;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
  this->swing.horizontal += rotation.swing.horizontal;
  this->swing.vertical += rotation.swing.vertical;
  this->twist += rotation.twist;
  return *this;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Inverse(SwingTwistOf<T> rotation) {
  SwingTwistOf<T> r = SwingTwistOf<T>(
      -rotation.swing.horizontal, -rotation.swing.vertical, -rotation.twist);
  return r;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::AngleAxis(float angle,
                                           const DirectionOf<T>& axis) {
  Vector3 axis_vector = axis.ToVector3();
  Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
  SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
  return r;
}

template <typename T>
AngleOf<T> SwingTwistOf<T>::Angle(const SwingTwistOf<T>& r1,
                                  const SwingTwistOf<T>& r2) {
  Quaternion q1 = r1.ToQuaternion();
  Quaternion q2 = r2.ToQuaternion();
  float angle = Quaternion::Angle(q1, q2);
  return AngleOf<T>::Degrees(angle);
}

template <typename T>
void SwingTwistOf<T>::Normalize() {
  AngleOf<T> deg90 = AngleOf<T>::Degrees(90);
  AngleOf<T> deg180 = AngleOf<T>::Degrees(180);

  if (this->swing.vertical > deg90 || this->swing.vertical < -deg90) {
    this->swing.horizontal += deg180;
    this->swing.vertical = deg180 - this->swing.vertical;
    this->twist += deg180;
  }
}

template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;