RoboidControl-cpp/LinearAlgebra/Angle.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 "Angle.h"
#include <math.h>
#include "FloatSingle.h"

namespace LinearAlgebra {

//===== AngleSingle, AngleOf<float>

template <>
AngleOf<float> AngleOf<float>::Degrees(float degrees) {
  if (isfinite(degrees)) {
    while (degrees < -180)
      degrees += 360;
    while (degrees >= 180)
      degrees -= 360;
  }

  return AngleOf<float>(degrees);
}

template <>
AngleOf<float> AngleOf<float>::Radians(float radians) {
  if (isfinite(radians)) {
    while (radians <= -pi)
      radians += 2 * pi;
    while (radians > pi)
      radians -= 2 * pi;
  }

  return Binary(radians * Rad2Deg);
}

template <>
float AngleOf<float>::InDegrees() const {
  return this->value;
}

template <>
float AngleOf<float>::InRadians() const {
  return this->value * Deg2Rad;
}

//===== Angle16, AngleOf<signed short>

template <>
AngleOf<signed short> AngleOf<signed short>::Degrees(float degrees) {
  // map float [-180..180) to integer [-32768..32767]
  signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F);
  return Binary(value);
}

template <>
AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
  if (!isfinite(radians))
    return AngleOf<signed short>::zero;

  // map float [-PI..PI) to integer [-32768..32767]
  signed short value = (signed short)roundf(radians / pi * 32768.0F);
  return Binary(value);
}

template <>
float AngleOf<signed short>::InDegrees() const {
  float degrees = this->value / 65536.0f * 360.0f;
  return degrees;
}

template <>
float AngleOf<signed short>::InRadians() const {
  float radians = this->value / 65536.0f * (2 * pi);
  return radians;
}

//===== Angle8, AngleOf<signed char>

template <>
AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
  // map float [-180..180) to integer [-128..127)
  signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
  return Binary(value);
}

template <>
AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
  if (!isfinite(radians))
    return AngleOf<signed char>::zero;

  // map float [-pi..pi) to integer [-128..127)
  signed char value = (signed char)roundf(radians / pi * 128.0f);
  return Binary(value);
}

template <>
float AngleOf<signed char>::InDegrees() const {
  float degrees = this->value / 256.0f * 360.0f;
  return degrees;
}

template <>
float AngleOf<signed char>::InRadians() const {
  float radians = this->value / 128.0f * pi;
  return radians;
}

//===== Generic

template <typename T>
AngleOf<T>::AngleOf() : value(0) {}

template <typename T>
AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}

template <typename T>
const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();

template <typename T>
AngleOf<T> AngleOf<T>::Binary(T rawValue) {
  AngleOf<T> angle = AngleOf<T>();
  angle.SetBinary(rawValue);
  return angle;
}

template <typename T>
T AngleOf<T>::GetBinary() const {
  return this->value;
}
template <typename T>
void AngleOf<T>::SetBinary(T rawValue) {
  this->value = rawValue;
}

template <typename T>
bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
  return this->value == angle.value;
}

template <typename T>
bool AngleOf<T>::operator>(AngleOf<T> angle) const {
  return this->value > angle.value;
}

template <typename T>
bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
  return this->value >= angle.value;
}

template <typename T>
bool AngleOf<T>::operator<(AngleOf<T> angle) const {
  return this->value < angle.value;
}

template <typename T>
bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
  return this->value <= angle.value;
}

template <typename T>
signed int AngleOf<T>::Sign(AngleOf<T> angle) {
  if (angle.value < 0)
    return -1;
  if (angle.value > 0)
    return 1;
  return 0;
}

template <typename T>
AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
  if (Sign(angle) < 0)
    return -angle;
  else
    return angle;
}

template <typename T>
AngleOf<T> AngleOf<T>::operator-() const {
  AngleOf<T> angle = Binary(-this->value);
  return angle;
}

template <>
AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& angle) const {
  AngleOf<float> r = Binary(this->value - angle.value);
  r = Normalize(r);
  return r;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator-(const AngleOf<T>& angle) const {
  AngleOf<T> r = Binary(this->value - angle.value);
  return r;
}

template <>
AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& angle) const {
  AngleOf<float> r = Binary(this->value + angle.value);
  r = Normalize(r);
  return r;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& angle) const {
  AngleOf<T> r = Binary(this->value + angle.value);
  return r;
}

template <>
AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& angle) {
  this->value += angle.value;
  this->Normalize();
  return *this;
}

template <typename T>
AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
  this->value += angle.value;
  return *this;
}

// This defintion is not matching the declaration in the header file somehow
// template <typename T>
// AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
//   return AngleOf::Degrees((float)angle.InDegrees() * factor);
// }

// This defintion is not matching the declaration in the header file somehow
// template <typename T>
// AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
//   return AngleOf::Degrees((float)factor * angle.InDegrees());
// }

template <typename T>
void AngleOf<T>::Normalize() {
  float angleValue = this->InDegrees();
  if (!isfinite(angleValue))
    return;

  while (angleValue <= -180)
    angleValue += 360;
  while (angleValue > 180)
    angleValue -= 360;
  *this = AngleOf::Degrees(angleValue);
}

template <typename T>
AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
  float angleValue = angle.InDegrees();
  if (!isfinite(angleValue))
    return angle;

  while (angleValue <= -180)
    angleValue += 360;
  while (angleValue > 180)
    angleValue -= 360;
  return AngleOf::Degrees(angleValue);
}

template <typename T>
AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
  float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
  return AngleOf<T>::Degrees(r);
}

template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
                                   AngleOf<T> toAngle,
                                   float maxDegrees) {
  maxDegrees = fmaxf(0, maxDegrees);  // filter out negative distances
  AngleOf<T> d = toAngle - fromAngle;
  float dDegrees = Abs(d).InDegrees();
  d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
  if (Sign(d) < 0)
    d = -d;

  return fromAngle + d;
}

template <typename T>
float AngleOf<T>::Cos(AngleOf<T> angle) {
  return cosf(angle.InRadians());
}
template <typename T>
float AngleOf<T>::Sin(AngleOf<T> angle) {
  return sinf(angle.InRadians());
}
template <typename T>
float AngleOf<T>::Tan(AngleOf<T> angle) {
  return tanf(angle.InRadians());
}

template <typename T>
AngleOf<T> AngleOf<T>::Acos(float f) {
  return AngleOf<T>::Radians(acosf(f));
}
template <typename T>
AngleOf<T> AngleOf<T>::Asin(float f) {
  return AngleOf<T>::Radians(asinf(f));
}
template <typename T>
AngleOf<T> AngleOf<T>::Atan(float f) {
  return AngleOf<T>::Radians(atanf(f));
}

template <typename T>
AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
  return AngleOf<T>::Radians(atan2f(y, x));
}

// template <>
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
// {
//   float a2 = a * a;
//   float b2 = b * b;
//   float d =
//       a2 + b2 -
//       2 * a * b * Cos(gamma);  // cosf(gamma *
//       Passer::LinearAlgebra::Deg2Rad);
//   // Catch edge cases where float inacuracies lead tot nans
//   if (d < 0)
//     return 0.0f;

//   float c = sqrtf(d);
//   return c;
// }

template <typename T>
float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
  float a2 = a * a;
  float b2 = b * b;
  float d =
      a2 + b2 -
      2 * a * b * Cos(gamma);  // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
  // Catch edge cases where float inacuracies lead tot nans
  if (d < 0)
    return 0;

  float c = sqrtf(d);
  return c;
}

// template <>
// AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
//   float a2 = a * a;
//   float b2 = b * b;
//   float c2 = c * c;
//   float d = (a2 + b2 - c2) / (2 * a * b);
//   // Catch edge cases where float inacuracies lead tot nans
//   if (d >= 1)
//     return 0.0f;
//   if (d <= -1)
//     return 180.0f;

//   float gamma = acosf(d) * Rad2Deg;
//   return gamma;
// }
template <typename T>
AngleOf<T> AngleOf<T>::CosineRuleAngle(float a, float b, float c) {
  float a2 = a * a;
  float b2 = b * b;
  float c2 = c * c;
  float d = (a2 + b2 - c2) / (2 * a * b);
  // Catch edge cases where float inacuracies lead tot nans
  if (d >= 1)
    return AngleOf<T>();
  if (d <= -1)
    return AngleOf<T>::Degrees(180);

  // float gamma = acosf(d) * Rad2Deg;
  AngleOf<T> gamma = Acos(d);
  return gamma;
}

// template <>
// AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
//                                              AngleOf<float> beta,
//                                              float b) {
//   float deg2rad = Deg2Rad;
//   float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
//   return alpha;
// }
template <typename T>
AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
  // float deg2rad = Deg2Rad;
  // float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
  AngleOf<T> alpha = Asin(a * Sin(beta) / b);
  return alpha;
}

template class AngleOf<float>;
template class AngleOf<signed char>;
template class AngleOf<signed short>;

}  // namespace LinearAlgebra