Added permature Spherical type

Spherical.cpp

@@ -0,0 +1,28 @@
+#include "Spherical.h"
+
+#include <math.h>
+
+Spherical::Spherical() {
+  this->horizontalAngle = 0;
+  this->verticalAngle = 0;
+  this->distance = 0;
+}
+
+// Spherical::Spherical(float polarAngle, float elevationAngle, float distance)
+// {
+//   this->horizontalAngle = polarAngle;
+//   this->verticalAngle = elevationAngle;
+//   this->distance = distance;
+// }
+
+Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
+    : distance(distance), horizontalAngle(horizontalAngle),
+      verticalAngle(verticalAngle) {}
+
+const Spherical Spherical::zero = Spherical(0, 0, 0);
+
+float Spherical::GetSwing() {
+  // Not sure if this is correct
+  return sqrtf(horizontalAngle * horizontalAngle +
+               verticalAngle * verticalAngle);
+}

Spherical.h

@@ -0,0 +1,46 @@
+/// @copyright
+/// 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/.
+
+#ifndef SPHERICAL_H
+#define SPHERICAL_H
+
+typedef float Angle;
+
+/// @brief A spherical vector
+/// @details
+/// This is a vector in 3D space using a spherical coordinate system.
+/// It consists of a distance and the polar and elevation angles from a
+/// reference direction. The reference direction is typically thought of
+/// as a forward direction.
+struct Spherical {
+public:
+  float distance;
+
+  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
+  /// @details The angle is automatically normalized to -180 .. 180
+  Angle horizontalAngle;
+  /// @brief The angle in the vertical plane in degrees. Positive is upward.
+  /// @details The angle is automatically normalized to -180 .. 180
+  Angle verticalAngle;
+
+  /// @brief Create a new spherical vector with zero degrees and distance
+  Spherical();
+  /// @brief Create a new spherical vector
+  /// @param polarAngle The angle in the horizontal plane in degrees,
+  /// clockwise rotation
+  /// @param elevationAngle The angle in the vertical plan in degrees,
+  /// zero is forward, positive is upward
+  /// @param distance The distance in meters
+  //   Spherical(float polarAngle, float elevationAngle, float distance);
+
+  Spherical(float distance, Angle horizontalAngle, Angle verticalAngle);
+
+  /// @brief A spherical vector with zero degree angles and distance
+  const static Spherical zero;
+
+  float GetSwing();
+};
+
+#endif