Add source file

src/Polar.cpp

@@ -0,0 +1,69 @@
+#include <math.h>
+
+#include "Angle.h"
+#include "Polar.h"
+
+Polar::Polar() {
+  angle = 0.0F;
+  distance = 0.0F;
+}
+
+Polar::Polar(float newAngle, float newDistance) {
+  // distance should always be 0 or greater
+  if (newDistance < 0) {
+    angle = angle - 180;
+    distance = -newDistance;
+  } else {
+    angle = newAngle;
+    distance = newDistance;
+  }
+}
+
+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;
+}
+
+Polar Polar::operator+(const Polar& v2) const {
+  float deltaAngle = Angle::Normalize(v2.angle - this->angle);
+  float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
+
+  if (rotation == 180 && v2.distance > 0)
+    return Polar::zero;
+
+  // return Polar(this->angle + v2.angle, this->distance + v2.distance);
+  float newDistance =
+      Angle::CosineRuleSide(v2.distance, this->distance, rotation);
+
+  float angle = rotation;
+  if (newDistance > 0.001F &&
+      this->distance > 0.001F)  // nonzero because float inaccuracies may still
+                                // lead to problems with small number
+                                // plus, it is more efficient....
+    angle = Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
+  float newAngle = deltaAngle < 0 ? Angle::Normalize(this->angle - angle)
+                                  : Angle::Normalize(this->angle + angle);
+  Polar vector = Polar(newAngle, newDistance);
+  return vector;
+}
+
+Polar Polar::operator-() {
+  Polar vector = Polar(this->angle - 180, this->distance);
+  return vector;
+}
+
+Polar Polar::operator-(const Polar& v2) const {
+  Polar vector = *this + (Polar(v2.angle - 180, v2.distance));
+  return vector;
+}
+
+Polar Polar::operator*(float f) const {
+  return Polar(this->angle, this->distance * f);
+}
+
+Polar Polar::operator/(const float& f) {
+  return Polar(this->angle, this->distance / f);
+}