RoboidControl-cpp/LinearAlgebra/Polar.cpp

#include <math.h>

#include "Polar.h"
#include "Vector2.h"

template <typename T>
PolarOf<T>::PolarOf() {
  this->distance = 0.0f;
  this->angle = AngleOf<T>();
}
template <typename T>
PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
  // distance should always be 0 or greater
  if (distance < 0.0f) {
    this->distance = -distance;
    this->angle = AngleOf<T>::Normalize(angle - AngleOf<T>::Degrees(180));
  } else {
    this->distance = distance;
    if (this->distance == 0.0f)
      // angle is always 0 if distance is 0
      this->angle = AngleOf<T>();
    else
      this->angle = AngleOf<T>::Normalize(angle);
  }
}

template <typename T>
PolarOf<T> PolarOf<T>::Degrees(float distance, float degrees) {
  AngleOf<T> angle = AngleOf<T>::Degrees(degrees);
  PolarOf<T> r = PolarOf<T>(distance, angle);
  return r;
}

template <typename T>
PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
  return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
}

template <typename T>
PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
  float distance = v.magnitude();
  AngleOf<T> angle =
      AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
  PolarOf<T> p = PolarOf(distance, angle);
  return p;
}
template <typename T>
PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
  float distance =
      v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
  AngleOf<T> angle = v.direction.horizontal;
  PolarOf<T> p = PolarOf(distance, angle);
  return p;
}

template <typename T>
const PolarOf<T> PolarOf<T>::zero = PolarOf(0.0f, AngleOf<T>());
template <typename T>
const PolarOf<T> PolarOf<T>::forward = PolarOf(1.0f, AngleOf<T>());
template <typename T>
const PolarOf<T> PolarOf<T>::back = PolarOf(1.0, AngleOf<T>::Degrees(180));
template <typename T>
const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
template <typename T>
const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));

template <typename T>
bool PolarOf<T>::operator==(const PolarOf& v) const {
  return (this->distance == v.distance &&
          this->angle.InDegrees() == v.angle.InDegrees());
}

template <typename T>
PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
  PolarOf<T> r = PolarOf(1, v.angle);
  return r;
}
template <typename T>
PolarOf<T> PolarOf<T>::normalized() const {
  PolarOf<T> r = PolarOf(1, this->angle);
  return r;
}

template <typename T>
PolarOf<T> PolarOf<T>::operator-() const {
  PolarOf<T> v =
      PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
  return v;
}

template <typename T>
PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
  PolarOf<T> r = -v;
  return *this + r;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
  *this = *this - v;
  return *this;
  // angle = AngleOf<T>::Normalize(newAngle);
  // distance = newDistance;
}

// Polar::Polar(Vector2 v) {
//   float signY = (v.y >= 0) - (v.y < 0);
//   angle = atan2(v.y, signY * sqrt(v.y * v.y + v.x * v.x)) * Angle::Rad2Deg;
//   distance = v.magnitude();
// }

// const Polar Polar::zero = Polar(0, 0);

// float Polar::Distance(const Polar &v1, const Polar &v2) {
//   float d =
//       Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
//   return d;
// }

template <typename T>
PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
  if (v.distance == 0)
    return PolarOf(this->distance, this->angle);
  if (this->distance == 0.0f)
    return v;

  float deltaAngle = AngleOf<T>::Normalize(v.angle - this->angle).InDegrees();
  float rotation =
      deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;

  if (rotation == 180.0f && v.distance > 0.0f) {
    //  angle is too small, take this angle and add the distances
    return PolarOf(this->distance + v.distance, this->angle);
  }

  float newDistance = AngleOf<T>::CosineRuleSide(v.distance, this->distance,
                                                 AngleOf<T>::Degrees(rotation));

  float angle =
      AngleSingle::CosineRuleAngle(newDistance, this->distance, v.distance)
          .InDegrees();

  float newAngle = deltaAngle < 0.0f ? this->angle.InDegrees() - angle
                                     : this->angle.InDegrees() + angle;
  AngleOf<T> newAngleA = AngleOf<T>::Normalize(AngleOf<T>::Degrees(newAngle));
  PolarOf vector = PolarOf(newDistance, newAngleA);
  return vector;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
  *this = *this + v;
  return *this;
}

template <typename T>
PolarOf<T> PolarOf<T>::operator*=(float f) {
  this->distance *= f;
  return *this;
}
template <typename T>
PolarOf<T> PolarOf<T>::operator/=(float f) {
  this->distance /= f;
  return *this;
}

template <typename T>
float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
  float d =
      AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
  return d;
}

template <typename T>
PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
  AngleOf<T> a = AngleOf<T>::Normalize(v.angle + angle);
  PolarOf<T> r = PolarOf(v.distance, a);
  return r;
}

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