diff --git a/Spherical.cpp b/Spherical.cpp index 5efef69..b521c77 100644 --- a/Spherical.cpp +++ b/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, diff --git a/Spherical.h b/Spherical.h index dcad9f7..3bbf72e 100644 --- a/Spherical.h +++ b/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: /// The distance between the two vectors // 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); diff --git a/test/Polar_test.cc b/test/Polar_test.cc index 1c48cd7..8dfc18d 100644 --- a/test/Polar_test.cc +++ b/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) { diff --git a/test/Spherical_test.cc b/test/Spherical_test.cc index df8f18a..5e89ce2 100644 --- a/test/Spherical_test.cc +++ b/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 \ No newline at end of file