Added Spherical16

CMakeLists.txt

@@ -33,6 +33,7 @@ else()
         "Quaternion.cpp"
         "Polar.cpp" 
         "Spherical.cpp"
+        "Spherical16.cpp"
         "Matrix.cpp"
         "Axis.cpp"
         "AngleAxis.cpp"
@@ -50,6 +51,7 @@ else()
         "test/Matrix_test.cc"
         "test/Polar_test.cc"
         "test/Spherical_test.cc"
+        "test/Spherical16_test.cc"
         "test/DiscreteAngle_test.cc"
     )
     target_link_libraries(

Spherical16.cpp

@@ -0,0 +1,232 @@
+#include "Spherical16.h"
+
+#include "Quaternion.h"
+#include "Spherical.h"
+
+#include <math.h>
+
+Spherical16::Spherical16() {
+  this->distance = 0.0f;
+  this->horizontalAngle = Angle16(0);
+  this->verticalAngle = Angle16(0);
+}
+
+Spherical16::Spherical16(Polar polar) {
+  this->distance = polar.distance;
+  this->horizontalAngle = Angle16(polar.angle);
+  this->verticalAngle = Angle16(0);
+}
+
+Spherical16::Spherical16(float distance,
+                         Angle16 horizontalAngle,
+                         Angle16 verticalAngle) {
+  if (distance < 0) {
+    this->distance = -distance;
+    this->horizontalAngle = horizontalAngle - Angle16(180);
+    this->verticalAngle = verticalAngle;
+  } else {
+    this->distance = distance;
+    this->horizontalAngle = horizontalAngle;
+    this->verticalAngle = verticalAngle;
+  }
+}
+
+Spherical16::Spherical16(Vector3 v) {
+  this->distance = v.magnitude();
+  if (distance == 0.0f) {
+    this->verticalAngle = 0.0f;
+    this->horizontalAngle = 0.0f;
+  } else {
+    this->verticalAngle =
+        (90.0f - acosf(v.Up() / this->distance) * Angle::Rad2Deg);
+    this->horizontalAngle = atan2f(v.Right(), v.Forward()) * Angle::Rad2Deg;
+  }
+}
+
+const Spherical16 Spherical16::zero = Spherical16(0.0f, 0.0f, 0.0f);
+const Spherical16 Spherical16::forward = Spherical16(1.0f, 0.0f, 0.0f);
+const Spherical16 Spherical16::back = Spherical16(1.0f, 180.0f, 0.0f);
+const Spherical16 Spherical16::right = Spherical16(1.0f, 90.0f, 0.0f);
+const Spherical16 Spherical16::left = Spherical16(1.0f, -90.0f, 0.0f);
+const Spherical16 Spherical16::up = Spherical16(1.0f, 0.0f, 90.0f);
+const Spherical16 Spherical16::down = Spherical16(1.0f, 0.0f, -90.0f);
+
+bool Spherical16::operator==(const Spherical16& v) const {
+  return (this->distance == v.distance &&
+          this->horizontalAngle == v.horizontalAngle &&
+          this->verticalAngle == v.verticalAngle);
+}
+
+Spherical16 Spherical16::Normalize(const Spherical16& v) {
+  Spherical16 r = Spherical16(1, v.horizontalAngle, v.verticalAngle);
+  return r;
+}
+Spherical16 Spherical16::normalized() const {
+  Spherical16 r = Spherical16(1, this->horizontalAngle, this->verticalAngle);
+  return r;
+}
+
+Spherical16 Spherical16::operator-() const {
+  Spherical16 v = Spherical16(this->distance, this->horizontalAngle + 180.0f,
+                              this->verticalAngle + 180.0f);
+  return v;
+}
+
+Spherical16 Spherical16::operator-(const Spherical16& s2) const {
+  Spherical thisSpherical = Spherical(
+      this->distance, (Angle)this->horizontalAngle, (Angle)this->verticalAngle);
+  Spherical spherical2 = Spherical(s2.distance, (Angle)s2.horizontalAngle,
+                                   (Angle)s2.verticalAngle);
+
+  // let's do it the easy way...
+  Vector3 v1 = Vector3(thisSpherical);
+  Vector3 v2 = Vector3(spherical2);
+  Vector3 v = v1 - v2;
+  Spherical16 r = Spherical16(v);
+  return r;
+}
+Spherical16 Spherical16::operator-=(const Spherical16& v) {
+  *this = *this - v;
+  return *this;
+}
+
+Spherical16 Spherical16::operator+(const Spherical16& s2) const {
+  // let's do it the easy way...
+  Vector3 v1 = Vector3(Spherical(this->distance, (float)this->horizontalAngle,
+                                 (float)this->verticalAngle));
+  Vector3 v2 = Vector3(Spherical(s2.distance, (float)s2.horizontalAngle,
+                                 (float)s2.verticalAngle));
+  Vector3 v = v1 + v2;
+  Spherical16 r = Spherical16(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);
+  */
+}
+Spherical16 Spherical16::operator+=(const Spherical16& v) {
+  *this = *this + v;
+  return *this;
+}
+
+// Spherical Passer::LinearAlgebra::operator*(const Spherical &v, float f) {
+//   return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
+// }
+// Spherical Passer::LinearAlgebra::operator*(float f, const Spherical &v) {
+//   return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
+// }
+Spherical16 Spherical16::operator*=(float f) {
+  this->distance *= f;
+  return *this;
+}
+
+// Spherical Passer::LinearAlgebra::operator/(const Spherical &v, float f) {
+//   return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
+// }
+// Spherical Passer::LinearAlgebra::operator/(float f, const Spherical &v) {
+//   return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
+// }
+Spherical16 Spherical16::operator/=(float f) {
+  this->distance /= f;
+  return *this;
+}
+
+// float Spherical::GetSwing() {
+//   // Not sure if this is correct
+//   return sqrtf(horizontalAngle * horizontalAngle +
+//                verticalAngle * verticalAngle);
+// }
+
+// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
+//   float d = 0;
+//   return d;
+// }
+
+#include "AngleUsing.h"
+#include "FloatSingle.h"
+#include "Vector3.h"
+
+const float epsilon = 1E-05f;
+const float Rad2Deg = 57.29578F;
+
+Angle Spherical16::AngleBetween(const Spherical16& v1, const Spherical16& 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.0f;
+
+  Vector3 v1_3 = Vector3(Spherical(v1.distance, (Angle)v1.horizontalAngle,
+                                   (Angle)v1.verticalAngle));
+  Vector3 v2_3 = Vector3(Spherical(v2.distance, (Angle)v2.horizontalAngle,
+                                   (Angle)v2.verticalAngle));
+  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;
+}
+
+Spherical16 Spherical16::Rotate(const Spherical16& v,
+                                Angle horizontalAngle,
+                                Angle verticalAngle) {
+  Spherical16 r = Spherical16(v.distance, v.horizontalAngle + horizontalAngle,
+                              v.verticalAngle + verticalAngle);
+  return r;
+}
+Spherical16 Spherical16::RotateHorizontal(const Spherical16& v, Angle a) {
+  Spherical16 r =
+      Spherical16(v.distance, v.horizontalAngle + a, v.verticalAngle);
+  return r;
+}
+Spherical16 Spherical16::RotateVertical(const Spherical16& v, Angle a) {
+  Spherical16 r =
+      Spherical16(v.distance, v.horizontalAngle, v.verticalAngle + a);
+  return r;
+}

Spherical16.h

@@ -0,0 +1,158 @@
+// 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 SPHERICAL16_H
+#define SPHERICAL16_H
+
+#include "Angle16.h"
+#include "Polar.h"
+
+namespace Passer {
+namespace LinearAlgebra {
+
+struct Vector3;
+
+/// @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.
+/// In contrast to the normal Spherical type, this version uses 16 bit integers
+/// for the angles
+struct Spherical16 {
+ public:
+  /// @brief The distance in meters
+  /// @remark The distance should never be negative
+  float distance;
+  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
+  /// @details The angle is automatically normalized to -180 .. 180
+  Angle16 horizontalAngle;
+  /// @brief The angle in the vertical plane in degrees. Positive is upward.
+  /// @details The angle is automatically normalized to -180 .. 180
+  Angle16 verticalAngle;
+
+  /// @brief Create a new spherical vector with zero degrees and distance
+  Spherical16();
+  /// @brief Create a new spherical vector
+  /// @param distance The distance in meters
+  /// @param horizontalAngle The angle in the horizontal plane in degrees,
+  /// clockwise rotation
+  /// @param verticalAngle The angle in the vertical plan in degrees,
+  /// zero is forward, positive is upward
+  Spherical16(float distance, Angle16 horizontalAngle, Angle16 verticalAngle);
+  /// @brief Convert polar coordinates to spherical coordinates
+  /// @param polar The polar coordinate
+  Spherical16(Polar polar);
+  /// @brief Convert 3D carthesian coordinates to spherical coordinates
+  /// @param v Vector in 3D carthesian coordinates;
+  Spherical16(Vector3 v);
+
+  /// @brief A spherical vector with zero degree angles and distance
+  const static Spherical16 zero;
+  /// @brief A normalized forward-oriented vector
+  const static Spherical16 forward;
+  /// @brief A normalized back-oriented vector
+  const static Spherical16 back;
+  /// @brief A normalized right-oriented vector
+  const static Spherical16 right;
+  /// @brief A normalized left-oriented vector
+  const static Spherical16 left;
+  /// @brief A normalized up-oriented vector
+  const static Spherical16 up;
+  /// @brief A normalized down-oriented vector
+  const static Spherical16 down;
+
+  /// @brief Equality test to another vector
+  /// @param v The vector to check against
+  /// @return true: if it is identical to the given vector
+  /// @note This uses float comparison to check equality which may have strange
+  /// effects. Equality on floats should be avoided.
+  bool operator==(const Spherical16& v) const;
+
+  /// @brief The vector length
+  /// @param v The vector for which you need the length
+  /// @return The vector length;
+  inline static float Magnitude(const Spherical16& v) { return v.distance; }
+  /// @brief The vector length
+  /// @return The vector length
+  inline float magnitude() const { return this->distance; }
+
+  /// @brief Convert the vector to a length of 1
+  /// @param v The vector to convert
+  /// @return The vector normalized to a length of 1
+  static Spherical16 Normalize(const Spherical16& v);
+  /// @brief Convert the vector to a length of a
+  /// @return The vector normalized to a length of 1
+  Spherical16 normalized() const;
+
+  /// @brief Negate the vector
+  /// @return The negated vector
+  /// This will rotate the vector by 180 degrees horizontally and
+  /// vertically. Distance will stay the same.
+  Spherical16 operator-() const;
+
+  /// @brief Subtract a spherical vector from this vector
+  /// @param v The vector to subtract
+  /// @return The result of the subtraction
+  Spherical16 operator-(const Spherical16& v) const;
+  Spherical16 operator-=(const Spherical16& v);
+  /// @brief Add a spherical vector to this vector
+  /// @param v The vector to add
+  /// @return The result of the addition
+  Spherical16 operator+(const Spherical16& v) const;
+  Spherical16 operator+=(const Spherical16& v);
+
+  /// @brief Scale the vector uniformly up
+  /// @param f The scaling factor
+  /// @return The scaled vector
+  /// @remark This operation will scale the distance of the vector. The angle
+  /// will be unaffected.
+  friend Spherical16 operator*(const Spherical16& v, float f) {
+    return Spherical16(v.distance * f, v.horizontalAngle, v.verticalAngle);
+  }
+  friend Spherical16 operator*(float f, const Spherical16& v) {
+    return Spherical16(
+        v.distance * f, v.horizontalAngle,
+        v.verticalAngle);  // not correct, should be f * v.distance
+  }
+  Spherical16 operator*=(float f);
+  /// @brief Scale the vector uniformly down
+  /// @param f The scaling factor
+  /// @return The scaled factor
+  /// @remark This operation will scale the distance of the vector. The angle
+  /// will be unaffected.
+  friend Spherical16 operator/(const Spherical16& v, float f) {
+    return Spherical16(v.distance / f, v.horizontalAngle, v.verticalAngle);
+  }
+  friend Spherical16 operator/(float f, const Spherical16& v) {
+    return Spherical16(
+        v.distance / f, v.horizontalAngle,
+        v.verticalAngle);  // not correct, should be f / v.distance
+  }
+  Spherical16 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 Spherical16 &s1, const Spherical16 &s2);
+
+  static Angle AngleBetween(const Spherical16& v1, const Spherical16& v2);
+
+  static Spherical16 Rotate(const Spherical16& v,
+                            Angle horizontalAngle,
+                            Angle verticalAngle);
+  static Spherical16 RotateHorizontal(const Spherical16& v, Angle angle);
+  static Spherical16 RotateVertical(const Spherical16& v, Angle angle);
+};
+
+}  // namespace LinearAlgebra
+}  // namespace Passer
+using namespace Passer::LinearAlgebra;
+
+#include "Vector3.h"
+
+#endif

test/Spherical16_test.cc

@@ -0,0 +1,160 @@
+#if GTEST
+#include <gtest/gtest.h>
+#include <math.h>
+#include <limits>
+
+#include "Spherical16.h"
+
+#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
+
+TEST(Spherical16, FromVector3) {
+  Vector3 v = Vector3(0, 0, 1);
+  Spherical16 s = Spherical16(v);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
+  EXPECT_FLOAT_EQ((float)s.horizontalAngle, 0.0F) << "s.hor 0 0 1";
+  EXPECT_FLOAT_EQ((float)s.verticalAngle, 0.0F) << "s.vert 0 0 1";
+
+  v = Vector3(0, 1, 0);
+  s = Spherical16(v);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, 0.0F) << "s.hor 0 1 0";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 90.0F) << "s.vert 0 1 0";
+
+  v = Vector3(1, 0, 0);
+  s = Spherical16(v);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, 90.0F) << "s.hor 1 0 0";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert 1 0 0";
+}
+
+TEST(Spherical16, FromPolar) {
+  Polar p = Polar(1, 0);
+  Spherical16 s = Spherical16(p);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, 0.0F) << "s.hor Polar(1 0)";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert Polar(1 0)";
+
+  p = Polar(1, 45);
+  s = Spherical16(p);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, 45.0F) << "s.hor Polar(1 45)";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert Polar(1 45)";
+
+  p = Polar(1, -45);
+  s = Spherical16(p);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, -45.0F) << "s.hor Polar(1 -45)";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert Polar(1 -45)";
+
+  p = Polar(0, 0);
+  s = Spherical16(p);
+
+  EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, 0.0F) << "s.hor Polar(0 0)";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert Polar(0 0)";
+
+  p = Polar(-1, 0);
+  s = Spherical16(p);
+
+  EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
+  EXPECT_FLOAT_EQ(s.horizontalAngle, -180.0F) << "s.hor Polar(-1 0)";
+  EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert Polar(-1 0)";
+}
+
+TEST(Spherical16, Incident1) {
+  Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
+  Spherical16 s = Spherical16(v);
+
+  Spherical16 sr = Spherical16(2.49F, 98.18f, 24.4F);
+  EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
+  EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-02);
+  EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-02);
+
+  Vector3 r = Vector3(Spherical(sr.distance, (Angle)sr.horizontalAngle,
+                                (Angle)sr.verticalAngle));
+  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
+  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
+  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
+}
+
+TEST(Spherical16, Incident2) {
+  Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
+  Spherical16 s = Spherical16(v);
+
+  Spherical16 sr = Spherical16(1.4142135623F, 45.0f, 0.0F);
+  EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
+  EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-05);
+  EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-05);
+
+  Vector3 r = Vector3(Spherical(sr.distance, (Angle)sr.horizontalAngle,
+                                (Angle)sr.verticalAngle));
+  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
+  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
+  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
+
+  v = Vector3(0.0f, 1.0f, 1.0f);
+  s = Spherical16(v);
+
+  sr = Spherical16(1.4142135623F, 0.0f, 45.0F);
+  EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
+  EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-05);
+  EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-05);
+
+  r = Vector3(Spherical(sr.distance, (Angle)sr.horizontalAngle,
+                        (Angle)sr.verticalAngle));
+  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
+  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
+  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
+
+  v = Vector3(1.0f, 1.0f, 1.0f);
+  s = Spherical16(v);
+  r = Vector3(
+      Spherical(s.distance, (Angle)s.horizontalAngle, (Angle)s.verticalAngle));
+
+  EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
+  EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-02);
+  EXPECT_NEAR(s.verticalAngle, 35.26F, 1.0e-02);
+
+  EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
+  EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
+  EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-04);
+
+  // s = Spherical16(10, 45, 45);
+  // r = s.ToVector3();
+  // EXPECT_NEAR(r.x, 5, 1.0e-06);
+  // EXPECT_NEAR(r.y, 7.07, 1.0e-06);
+  // EXPECT_NEAR(r.z, 5, 1.0e-06);
+}
+
+TEST(Spherical16, Addition) {
+  Spherical16 v1 = Spherical16(1, 45, 0);
+  Spherical16 v2 = Spherical16::zero;
+  Spherical16 r = Spherical16::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 = Spherical16(1, -45, 0);
+  r = v1 + v2;
+  EXPECT_NEAR(r.distance, sqrtf(2), 1.0e-02) << "Addition(1 -45 0)";
+  EXPECT_FLOAT_EQ(r.horizontalAngle, 0) << "Addition(1 -45 0)";
+  EXPECT_FLOAT_EQ(r.verticalAngle, 0) << "Addition(1 -45 0)";
+
+  v2 = Spherical16(1, 0, 90);
+  r = v1 + v2;
+  EXPECT_NEAR(r.distance, sqrtf(2), 1.0e-02) << "Addition(1 0 90)";
+  EXPECT_NEAR(r.horizontalAngle, 45, 1.0e-01) << "Addition(1 0 90)";
+  EXPECT_NEAR(r.verticalAngle, 45, 1.0e-02) << "Addition(1 0 90)";
+}
+
+#endif