RoboidControl-cpp/Polar.cpp

#include <math.h>

#include "Polar.h"

template <typename T>
PolarOf<T>::PolarOf() {
  this->distance = 0.0f;
  this->angle = 0.0f;
}
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.ToFloat() - 180.0f);
  } else {
    this->distance = distance;
    if (this->distance == 0.0f)
      // angle is always 0 if distance is 0
      this->angle = 0.0f;
    else
      this->angle = AngleOf<T>::Normalize(angle);
  }
}

template <typename T>
PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
  float distance = v.magnitude();
  AngleOf<T> angle = 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.vertical.ToFloat() * Passer::LinearAlgebra::Deg2Rad);
  AngleOf<T> angle = v.horizontal;
  PolarOf<T> p = PolarOf(distance, angle);
  return p;
}

template <typename T>
const PolarOf<T> PolarOf<T>::zero = PolarOf(0.0f, 0.0f);
template <typename T>
const PolarOf<T> PolarOf<T>::forward = PolarOf(1.0f, 0.0f);
template <typename T>
const PolarOf<T> PolarOf<T>::back = PolarOf(1.0, 180.0f);
template <typename T>
const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, 90.0f);
template <typename T>
const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, -90.0f);

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

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>(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;
}

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 =
      Angle::Normalize(v.angle.ToFloat() - this->angle.ToFloat()).ToFloat();
  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 =
      Angle::CosineRuleSide(v.distance, this->distance, rotation).ToFloat();

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

  float newAngle = deltaAngle < 0.0f ? this->angle.ToFloat() - angle
                                     : this->angle.ToFloat() + angle;
  newAngle = Angle::Normalize(newAngle).ToFloat();
  PolarOf vector = PolarOf(newDistance, newAngle);
  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 = Angle::CosineRuleSide(v1.distance, v2.distance,
                                  v2.angle.ToFloat() - v1.angle.ToFloat())
                .ToFloat();
  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>;

//=====================================
/*
Polar::Polar() {
  this->distance = 0.0f;
  this->angle = 0.0f;
}
Polar::Polar(float distance, Angle angle) {
  // distance should always be 0 or greater
  if (distance < 0.0f) {
    this->distance = -distance;
    this->angle = Angle::Normalize(angle.ToFloat() - 180.0f);
  } else {
    this->distance = distance;
    if (this->distance == 0.0f)
      // angle is always 0 if distance is 0
      this->angle = 0.0f;
    else
      this->angle = Angle::Normalize(angle);
  }
}
Polar::Polar(Vector2 v) {
  this->distance = v.magnitude();
  this->angle = Vector2::SignedAngle(Vector2::forward, v);
}
Polar::Polar(Spherical v) {
  this->distance = v.distance * cosf(v.verticalAngle.ToFloat() *
                                     Passer::LinearAlgebra::Deg2Rad);
  this->angle = v.horizontalAngle;
}

const Polar Polar::zero = Polar(0.0f, 0.0f);
const Polar Polar::forward = Polar(1.0f, 0.0f);
const Polar Polar::back = Polar(1.0, 180.0f);
const Polar Polar::right = Polar(1.0, 90.0f);
const Polar Polar::left = Polar(1.0, -90.0f);

bool Polar::operator==(const Polar& v) const {
  return (this->distance == v.distance &&
          this->angle.ToFloat() == v.angle.ToFloat());
}

Polar Polar::Normalize(const Polar& v) {
  Polar r = Polar(1, v.angle);
  return r;
}
Polar Polar::normalized() const {
  Polar r = Polar(1, this->angle);
  return r;
}

Polar Polar::operator-() const {
  Polar v = Polar(this->distance, this->angle.ToFloat() + 180.0f);
  return v;
}

Polar Polar::operator-(const Polar& v) const {
  Polar r = -v;
  return *this + r;
}
Polar Polar::operator-=(const Polar& v) {
  *this = *this - v;
  return *this;
}
Polar Polar::operator+(const Polar& v) const {
  if (v.distance == 0)
    return Polar(this->distance, this->angle);
  if (this->distance == 0.0f)
    return v;

  float deltaAngle =
      Angle::Normalize(v.angle.ToFloat() - this->angle.ToFloat()).ToFloat();
  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 Polar(this->distance + v.distance, this->angle);
  }

  float newDistance =
      Angle::CosineRuleSide(v.distance, this->distance, rotation).ToFloat();

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

  float newAngle = deltaAngle < 0.0f ? this->angle.ToFloat() - angle
                                     : this->angle.ToFloat() + angle;
  newAngle = Angle::Normalize(newAngle).ToFloat();
  Polar vector = Polar(newDistance, newAngle);
  return vector;
}
Polar Polar::operator+=(const Polar& v) {
  *this = *this + v;
  return *this;
}

// Polar Passer::LinearAlgebra::operator*(const Polar &v, float f) {
//   return Polar(v.distance * f, v.angle);
// }
// Polar Passer::LinearAlgebra::operator*(float f, const Polar &v) {
//   return Polar(v.distance * f, v.angle);
// }
Polar Polar::operator*=(float f) {
  this->distance *= f;
  return *this;
}
// Polar Passer::LinearAlgebra::operator/(const Polar& v, float f) {
//   return Polar(v.distance / f, v.angle);
// }
// Polar Passer::LinearAlgebra::operator/(float f, const Polar& v) {
//   return Polar(v.distance / f, v.angle);
// }
Polar Polar::operator/=(float f) {
  this->distance /= f;
  return *this;
}

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

Polar Polar::Rotate(const Polar& v, Angle angle) {
  Angle a = Angle::Normalize(v.angle.ToFloat() + angle.ToFloat());
  Polar r = Polar(v.distance, a);
  return r;
}
*/