Added spherical <-> polar convertions

Spherical.cpp

@@ -19,6 +19,12 @@ Spherical::Spherical() {
 //   this->distance = distance;
 // }
 
+Spherical::Spherical(Polar polar) {
+  this->horizontalAngle = polar.angle;
+  this->verticalAngle = 0.0F;
+  this->magnitude = polar.distance;
+}
+
 Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
     : magnitude(distance), horizontalAngle(horizontalAngle),
       verticalAngle(verticalAngle) {}
@@ -37,4 +43,8 @@ float Spherical::GetSwing() {
   // Not sure if this is correct
   return sqrtf(horizontalAngle * horizontalAngle +
                verticalAngle * verticalAngle);
+}
+
+Polar Spherical::ProjectOnHorizontalPlane() {
+  Polar(horizontalAngle, magnitude);
 }

Spherical.h

@@ -7,6 +7,7 @@
 #define SPHERICAL_H
 
 #include "Angle.h"
+#include "Polar.h"
 
 struct Vector3;
 
@@ -41,6 +42,10 @@ public:
 
   Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
 
+  /// @brief Convert polar coordinates to spherical coordinates
+  /// @param polar The polar coordinate
+  Spherical(Polar polar);
+
   /// @brief Convert 3D carthesian coordinates to spherical coordinates
   /// @param v Vector in 3D carthesian coordinates;
   Spherical(Vector3 v);
@@ -49,6 +54,8 @@ public:
   const static Spherical zero;
 
   float GetSwing();
+
+  Polar ProjectOnHorizontalPlane();
 };
 
 } // namespace Passer