Added spherical addition

Spherical.cpp

@@ -71,6 +71,65 @@ Spherical Spherical::operator-() const {
   return v;
 }
 
+Spherical Spherical::operator+(const Spherical &s2) const {
+  // let's do it the easy way...
+  Vector3 v1 = Vector3(*this);
+  Vector3 v2 = Vector3(s2);
+  Vector3 v = v1 + v2;
+  Spherical r = Spherical(v);
+  return r;
+  /*
+  // This is the hard way...
+  if (v2.distance <= 0)
+    return Spherical(this->distance, this->horizontalAngle,
+                     this->verticalAngle);
+  if (this->distance <= 0)
+    return v2;
+
+  float deltaHorizontalAngle =
+      (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
+  float horizontalRotation = deltaHorizontalAngle < 0
+                                 ? 180 + deltaHorizontalAngle
+                                 : 180 - deltaHorizontalAngle;
+  float deltaVerticalAngle =
+      Angle::Normalize(v2.verticalAngle - this->verticalAngle);
+  float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
+                                                  : 180 - deltaVerticalAngle;
+
+  if (horizontalRotation == 180 && verticalRotation == 180)
+    // angle is too small, take this angle and add the distances
+    return Spherical(this->distance + v2.distance, this->horizontalAngle,
+                     this->verticalAngle);
+
+  Angle rotation = AngleBetween(*this, v2);
+  float newDistance =
+      Angle::CosineRuleSide(v2.distance, this->distance, rotation);
+  float angle =
+      Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
+
+  // Now we have to project the angle to the horizontal and vertical planes...
+  // The axis for the angle is the cross product of the two spherical vectors
+  // (which function we do not have either...)
+  float horizontalAngle = 0;
+  float verticalAngle = 0;
+
+  float newHorizontalAngle =
+      deltaHorizontalAngle < 0
+          ? Angle::Normalize(this->horizontalAngle - horizontalAngle)
+          : Angle::Normalize(this->horizontalAngle + horizontalAngle);
+  float newVerticalAngle =
+      deltaVerticalAngle < 0
+          ? Angle::Normalize(this->verticalAngle - verticalAngle)
+          : Angle::Normalize(this->verticalAngle + verticalAngle);
+
+  Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
+  */
+}
+Spherical Spherical::operator+=(const Spherical &v) {
+  *this = *this + v;
+  return *this;
+}
+
 Spherical Passer::LinearAlgebra::operator*(const Spherical &v, float f) {
   return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
 }
@@ -103,6 +162,34 @@ Spherical Spherical::operator/=(float f) {
 //   return d;
 // }
 
+#include "AngleUsing.h"
+#include "FloatSingle.h"
+#include "Vector3.h"
+
+const float epsilon = 1E-05f;
+const float Rad2Deg = 57.29578F;
+
+Angle Spherical::AngleBetween(const Spherical &v1, const Spherical &v2) {
+
+  // float denominator = sqrtf(v1_3.sqrMagnitude() * v2_3.sqrMagnitude());
+  float denominator =
+      v1.distance * v2.distance; // sqrtf(v1.distance * v1.distance *
+                                 // v2.distance * v2.distance);
+  if (denominator < epsilon)
+    return 0;
+
+  Vector3 v1_3 = Vector3(v1);
+  Vector3 v2_3 = Vector3(v2);
+  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;
+}
+
 Spherical Spherical::Rotate(const Spherical &v, Angle horizontalAngle,
                             Angle verticalAngle) {
   Spherical r = Spherical(v.distance, v.horizontalAngle + horizontalAngle,

Spherical.h

@@ -90,6 +90,12 @@ public:
   /// vertically. Distance will stay the same.
   Spherical operator-() const;
 
+  /// @brief Add a spherical vector to this vector
+  /// @param v The vector to add
+  /// @return The result of the addition
+  Spherical operator+(const Spherical &v) const;
+  Spherical operator+=(const Spherical &v);
+
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
@@ -115,6 +121,8 @@ public:
   /// <returns>The distance between the two vectors</returns>
   // static float Distance(const Spherical &s1, const Spherical &s2);
 
+  static Angle AngleBetween(const Spherical &v1, const Spherical &v2);
+
   static Spherical Rotate(const Spherical &v, Angle horizontalAngle,
                           Angle verticalAngle);
   static Spherical RotateHorizontal(const Spherical &v, Angle angle);

test/Polar_test.cc

@@ -106,7 +106,7 @@ TEST(Polar, Subtraction) {
 }
 
 TEST(Polar, Addition) {
-  Polar v1 = Polar(4, 45);
+  Polar v1 = Polar(1, 45);
   Polar v2 = Polar(1, -90);
   Polar r = Polar::zero;
 
@@ -120,6 +120,11 @@ TEST(Polar, Addition) {
   r = v1;
   r += v2;
   EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";
+
+  v2 = Polar(1, -45);
+  r = v1 + v2;
+  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
+  EXPECT_FLOAT_EQ(r.angle, 0) << "Addition(0 0 0)";
 }
 
 TEST(Polar, Scale_Multiply) {

test/Spherical_test.cc

@@ -128,4 +128,29 @@ TEST(Spherical, Incident2) {
   // EXPECT_NEAR(r.z, 5, 1.0e-06);
 }
 
+TEST(Spherical, Addition) {
+  Spherical v1 = Spherical(1, 45, 0);
+  Spherical v2 = Spherical::zero;
+  Spherical r = Spherical::zero;
+
+  r = v1 + v2;
+  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
+
+  r = v1;
+  r += v2;
+  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
+
+  v2 = Spherical(1, -45, 0);
+  r = v1 + v2;
+  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
+  EXPECT_FLOAT_EQ(r.horizontalAngle, 0) << "Addition(0 0 0)";
+  EXPECT_FLOAT_EQ(r.verticalAngle, 0) << "Addition(0 0 0)";
+
+  v2 = Spherical(1, 0, 90);
+  r = v1 + v2;
+  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
+  EXPECT_FLOAT_EQ(r.horizontalAngle, 45) << "Addition(1 0 90)";
+  EXPECT_FLOAT_EQ(r.verticalAngle, 45) << "Addition(1 0 90)";
+}
+
 #endif