Added initial spherical implementation

include/Spherical.h

@@ -0,0 +1,42 @@
+/// @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
+
+/// @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:
+  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
+  /// @details The angle is automatically normalized to -180 .. 180
+  float polarAngle;
+  /// @brief The angle in the vertical plane in degrees. Positive is upward.
+  /// @details The angle is automatically normalized to -180 .. 180
+  float elevationAngle;
+
+  float distance;
+
+  /// @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);
+
+  /// @brief A spherical vector with zero degree angles and distance
+  const static Spherical zero;
+
+  float GetSwing();
+};
+
+#endif

src/Spherical.cpp

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