RoboidControl-cpp/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"

const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
/*
float Angle::Normalize(float angle) {
  if (!isfinite(angle))
    return angle;

  while (angle <= -180)
    angle += 360;
  while (angle > 180)
    angle -= 360;
  return angle;
}

float Angle::Clamp(float angle, float min, float max) {
  float normalizedAngle = Normalize(angle);
  float r = Float::Clamp(normalizedAngle, min, max);
  return r;
}

float Angle::Difference(float a, float b) {
  float r = Normalize(b - a);
  return r;
}

float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) {
  float d = toAngle - fromAngle;
  float sign = signbit(d) ? -1 : 1;
  d = sign * Float::Clamp(fabs(d), 0, maxAngle);
  return fromAngle + d;
}

float Angle::CosineRuleSide(float a, float b, float gamma) {
  float a2 = a * a;
  float b2 = b * b;
  float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad);
  // Catch edge cases where float inacuracies lead tot nans
  if (d < 0)
    return 0;

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

float Angle::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;
  if (d <= -1)
    return 180;

  float gamma = acos(d) * Angle::Rad2Deg;
  return gamma;
}

float Angle::SineRuleAngle(float a, float beta, float b) {
  float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
  return alpha;
}
*/
//----------------------

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

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

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

template <>
AngleOf<float> AngleOf<float>::Degrees(float angle) {
  return AngleOf(angle);
}

template <>
AngleOf<float> AngleOf<float>::Radians(float angle) {
  return AngleOf(angle * 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 angle) {
  if (!isfinite(angle)) {
    return AngleOf<signed short>(0);
  }

  // map float [-180..180) to integer [-32768..32767]
  signed short value = (signed short)(angle / 360.0F * 65536.0F);
  return AngleOf<signed short>(value);
}

template <>
AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
  if (!isfinite(angle)) {
    return AngleOf<signed short>(0);
  }

  // map float [-PI..PI) to integer [-32768..32767]
  signed short value = (signed short)(angle / pi * 32768.0F);
  return AngleOf<signed short>(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 angle) {
  if (!isfinite(angle))
    return AngleOf<signed char>(0);

  // map float [-180..180) to integer [-128..127)
  signed char value = (signed char)(angle / 360.0F * 256.0F);
  return AngleOf<signed char>(value);
}

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

  // map float [-pi..pi) to integer [-128..127)
  signed char value = (signed char)(angle / pi * 128.0f);
  return AngleOf<signed char>(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>
bool AngleOf<T>::operator==(const AngleOf<T> a) const {
  return this->value == a.value;
}

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

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

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

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

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

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

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

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

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 <>
AngleOf<float> AngleOf<float>::Clamp(AngleOf<float> angle,
                                     AngleOf<float> min,
                                     AngleOf<float> max) {
  float normalizedAngle = Normalize(angle).InDegrees();
  float r = Float::Clamp(normalizedAngle, min.InDegrees(), max.InDegrees());
  return r;
}

template <>
AngleOf<float> AngleOf<float>::MoveTowards(AngleOf<float> fromAngle,
                                           AngleOf<float> toAngle,
                                           AngleOf<float> maxAngle) {
  float d = toAngle.InDegrees() - fromAngle.InDegrees();
  int sign = signbit(d) ? -1 : 1;
  d = sign * Float::Clamp(fabsf(d), 0, maxAngle.InDegrees());
  return fromAngle.InDegrees() + d;
}

template <typename T>
float AngleOf<T>::Cos(AngleOf<T> a) {
  return cosf(a.InRadians());
}
template <typename T>
float AngleOf<T>::Sin(AngleOf<T> a) {
  return sinf(a.InRadians());
}
template <typename T>
float AngleOf<T>::Tan(AngleOf<T> a) {
  return tanf(a.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 <>
AngleOf<float> AngleOf<float>::CosineRuleSide(float a, float b, float gamma) {
  float a2 = a * a;
  float b2 = b * b;
  float d = a2 + b2 - 2 * a * b * 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 <>
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 <>
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 class AngleOf<float>;
template class AngleOf<signed char>;
template class AngleOf<signed short>;