Added angle and rotate functions

Spherical.cpp

@@ -203,6 +203,51 @@ SphericalOf<T> SphericalOf<T>::operator/=(float f) {
   return *this;
 }
 
+#include "FloatSingle.h"
+#include "Vector3.h"
+
+const float epsilon = 1E-05f;
+
+template <typename T>
+AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
+                                        const SphericalOf& v2) {
+  float denominator = v1.distance * v2.distance;
+  if (denominator < epsilon)
+    return 0.0f;
+
+  Vector3 v1_3 = v1.ToVector3();
+  Vector3 v2_3 = v2.ToVector3();
+  float dot = Vector3::Dot(v1_3, v2_3);
+  float fraction = dot / denominator;
+  if (isnan(fraction))
+    return fraction;  // short cut to returning NaN universally
+
+  float cdot = Float::Clamp(fraction, -1.0, 1.0);
+  float r = ((float)acos(cdot)) * Rad2Deg;
+  return r;
+}
+
+template <typename T>
+SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
+                                      AngleOf<T> horizontalAngle,
+                                      AngleOf<T> verticalAngle) {
+  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + horizontalAngle,
+                                 v.vertical + verticalAngle);
+  return r;
+}
+template <typename T>
+SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
+                                                AngleOf<T> a) {
+  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + a, v.vertical);
+  return r;
+}
+template <typename T>
+SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
+                                              AngleOf<T> a) {
+  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal, v.vertical + a);
+  return r;
+}
+
 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
 

Spherical.h

@@ -94,6 +94,25 @@ class SphericalOf {
                           v.vertical);  // not correct, should be f / v.distance
   }
   SphericalOf<T> operator/=(float f);
+
+  /// <summary>
+  /// The distance between two vectors
+  /// </summary>
+  /// <param name="v1">The first vector</param>
+  /// <param name="v2">The second vector</param>
+  /// <returns>The distance between the two vectors</returns>
+  // static float Distance(const Spherical &s1, const Spherical &s2);
+
+  static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
+                                 const SphericalOf<T>& v2);
+
+  static SphericalOf<T> Rotate(const SphericalOf& v,
+                               AngleOf<T> horizontalAngle,
+                               AngleOf<T> verticalAngle);
+  static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
+                                         AngleOf<T> angle);
+  static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
+                                       AngleOf<T> angle);
 };
 
 using SphericalSingle = SphericalOf<float>;