diff --git a/CMakeLists.txt b/CMakeLists.txt index bfa76be..2df11da 100644 --- a/CMakeLists.txt +++ b/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( diff --git a/Spherical16.cpp b/Spherical16.cpp new file mode 100644 index 0000000..b0761fb --- /dev/null +++ b/Spherical16.cpp @@ -0,0 +1,232 @@ +#include "Spherical16.h" + +#include "Quaternion.h" +#include "Spherical.h" + +#include + +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; +} \ No newline at end of file diff --git a/Spherical16.h b/Spherical16.h new file mode 100644 index 0000000..1f9f448 --- /dev/null +++ b/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); + + /// + /// The distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + // 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 \ No newline at end of file diff --git a/test/Spherical16_test.cc b/test/Spherical16_test.cc new file mode 100644 index 0000000..3dfac4c --- /dev/null +++ b/test/Spherical16_test.cc @@ -0,0 +1,160 @@ +#if GTEST +#include +#include +#include + +#include "Spherical16.h" + +#define FLOAT_INFINITY std::numeric_limits::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 \ No newline at end of file