From 92d5ef028caaa3ed7b36d89c046569f5ac7f43aa Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 23 Sep 2024 14:20:07 +0200 Subject: [PATCH] Normalizing directions --- Angle.cpp | 2 +- Angle.h | 2 +- CMakeLists.txt | 23 +++++++++-------- Direction.cpp | 50 ++++++++++++++++++++++++------------- Direction.h | 12 +++++++-- SwingTwist.cpp | 5 ++-- test/Direction_test.cc | 56 ++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 116 insertions(+), 34 deletions(-) create mode 100644 test/Direction_test.cc diff --git a/Angle.cpp b/Angle.cpp index 7f80414..efad991 100644 --- a/Angle.cpp +++ b/Angle.cpp @@ -112,7 +112,7 @@ float AngleOf::InRadians() const { // AngleOf AngleOf::Deg2Rad = (pi * 2) / 360.0f; template -bool AngleOf::operator==(AngleOf a) { +bool AngleOf::operator==(const AngleOf a) const { return this->value == a.value; } diff --git a/Angle.h b/Angle.h index 073d97a..c9a34ad 100644 --- a/Angle.h +++ b/Angle.h @@ -33,7 +33,7 @@ class AngleOf { // static AngleOf pi; - bool operator==(AngleOf a); + bool operator==(const AngleOf a) const; bool operator>(AngleOf a); bool operator>=(AngleOf a); bool operator<(AngleOf a); diff --git a/CMakeLists.txt b/CMakeLists.txt index ef13b2f..920de6d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,18 +43,21 @@ else() enable_testing() + file(GLOB_RECURSE test_srcs test/*.cc) add_executable( LinearAlgebraTest - "test/Angle_test.cc" - "test/FloatSingle_test.cc" - "test/Vector2_test.cc" - "test/Vector3_test.cc" - "test/Quaternion_test.cc" - "test/Matrix_test.cc" - "test/Polar_test.cc" - "test/Spherical_test.cc" - "test/Spherical16_test.cc" - "test/DiscreteAngle_test.cc" + ${test_srcs} + # "test/Angle_test.cc" + # "test/Direction_test.cc" + # "test/DiscreteAngle_test.cc" + # "test/FloatSingle_test.cc" + # "test/Matrix_test.cc" + # "test/Polar_test.cc" + # "test/Quaternion_test.cc" + # "test/Spherical_test.cc" + # "test/Spherical16_test.cc" + # "test/Vector2_test.cc" + # "test/Vector3_test.cc" ) target_link_libraries( LinearAlgebraTest diff --git a/Direction.cpp b/Direction.cpp index 0b77fe5..4641fd5 100644 --- a/Direction.cpp +++ b/Direction.cpp @@ -19,6 +19,7 @@ template DirectionOf::DirectionOf(AngleOf horizontal, AngleOf vertical) { this->horizontal = horizontal; this->vertical = vertical; + Normalize(); }; template @@ -26,6 +27,7 @@ DirectionOf::DirectionOf(Vector3 v) { this->horizontal = atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg; this->vertical = 90 - acosf(v.Up()) * Passer::LinearAlgebra::Rad2Deg; + Normalize(); } template @@ -41,28 +43,33 @@ const DirectionOf DirectionOf::left = DirectionOf(-90.0f, 0.0f); template const DirectionOf DirectionOf::right = DirectionOf(90.0f, 0.0f); +template +DirectionOf Passer::LinearAlgebra::DirectionOf::Degrees(float horizontal, + float vertical) { + return DirectionOf(AngleOf::Degrees(horizontal), + AngleOf::Degrees(vertical)); +} + +template +DirectionOf Passer::LinearAlgebra::DirectionOf::Radians(float horizontal, + float vertical) { + return DirectionOf(AngleOf::Radians(horizontal), + AngleOf::Radians(vertical)); +} + +template +bool Passer::LinearAlgebra::DirectionOf::operator==( + const DirectionOf d) const { + return (this->horizontal == d.horizontal) && (this->vertical == d.vertical); +} + template DirectionOf Passer::LinearAlgebra::DirectionOf::operator-() const { - DirectionOf r = DirectionOf(-this->horizontal, -this->vertical); + DirectionOf r = DirectionOf(this->horizontal + AngleOf::Degrees(180), + -this->vertical); return r; } -template -DirectionOf Passer::LinearAlgebra::DirectionOf::operator+( - const DirectionOf& v) const { - DirectionOf r = DirectionOf(this->horizontal + v.horizontal, - this->vertical + v.vertical); - return r; -} - -template -DirectionOf Passer::LinearAlgebra::DirectionOf::operator+=( - const DirectionOf& v) { - this->horizontal += v.horizontal; - this->vertical += v.vertical; - return *this; -} - template Vector3 DirectionOf::ToVector3() { Vector3 v = Quaternion::Euler(-(this->vertical.InDegrees()), @@ -71,5 +78,14 @@ Vector3 DirectionOf::ToVector3() { return v; } +template +void DirectionOf::Normalize() { + if (this->vertical > AngleOf::Degrees(90) || + this->vertical < AngleOf::Degrees(-90)) { + this->horizontal += AngleOf::Degrees(180); + this->vertical = AngleOf::Degrees(180) - this->vertical; + } +} + template class DirectionOf; template class DirectionOf; diff --git a/Direction.h b/Direction.h index 96d5bac..54faaac 100644 --- a/Direction.h +++ b/Direction.h @@ -15,13 +15,18 @@ struct Vector3; template class DirectionOf { public: + /// @brief horizontal angle, range= (-180..180] AngleOf horizontal; + /// @brief vertical angle, range in degrees = (-90..90] AngleOf vertical; DirectionOf(); DirectionOf(AngleOf horizontal, AngleOf vertical); DirectionOf(Vector3 v); + static DirectionOf Degrees(float horizontal, float vertical); + static DirectionOf Radians(float horizontal, float vertical); + const static DirectionOf forward; const static DirectionOf back; const static DirectionOf up; @@ -29,11 +34,14 @@ class DirectionOf { const static DirectionOf left; const static DirectionOf right; + bool operator==(const DirectionOf d) const; + DirectionOf operator-() const; - DirectionOf operator+(const DirectionOf& v) const; - DirectionOf operator+=(const DirectionOf& v); Vector3 ToVector3(); + + protected: + void Normalize(); }; using DirectionSingle = DirectionOf; diff --git a/SwingTwist.cpp b/SwingTwist.cpp index 7a7f4a8..af1b18c 100644 --- a/SwingTwist.cpp +++ b/SwingTwist.cpp @@ -79,9 +79,8 @@ SwingTwistOf SwingTwistOf::operator*( template SwingTwistOf SwingTwistOf::operator*=(const SwingTwistOf& rotation) { - // this->swing.horizontal += rotation.swing.horizontal; - // this->swing.vertical += rotation.swing.vertical; - this->swing += rotation.swing; + this->swing.horizontal += rotation.swing.horizontal; + this->swing.vertical += rotation.swing.vertical; this->twist += rotation.twist; return *this; } diff --git a/test/Direction_test.cc b/test/Direction_test.cc new file mode 100644 index 0000000..461140c --- /dev/null +++ b/test/Direction_test.cc @@ -0,0 +1,56 @@ +#if GTEST +#include + +#include +#include + +#include "Direction.h" + +#define FLOAT_INFINITY std::numeric_limits::infinity() + +TEST(Direction16, Compare) { + Direction16 d = Direction16::Degrees(45, 135); + bool r; + r = (d == Direction16(Angle16::Degrees(45), Angle16::Degrees(135))); + EXPECT_TRUE(r) << "45,135 == 45, 135"; + + r = (d == + Direction16(Angle16::Degrees(45 + 360), Angle16::Degrees(135 - 360))); + EXPECT_TRUE(r) << "45+360, 135-360 == 45, 135"; +} + +TEST(Direction16, Inverse) { + Direction16 d; + Direction16 r; + + d = Direction16::Degrees(45, 135); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(-135, -135)) << "-(45, 135)"; + + d = Direction16::Degrees(-45, -135); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(135, 135)) << "-(-45, -135)"; + + d = Direction16::Degrees(0, 0); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(180, 0)) << "-(0, 0)"; + + d = Direction16::Degrees(0, 45); + r = -d; + EXPECT_EQ(r, Direction16::Degrees(180, -45)) << "-(0, 45)"; +} + +TEST(Direction16, Equality) { + Direction16 d; + d = Direction16::Degrees(135, 45); + EXPECT_EQ(d, Direction16::Degrees(135, 45)) << "(135, 45) == (135, 45)"; + EXPECT_EQ(d, Direction16::Degrees(135 + 360, 45)) + << "(135, 45) == (135 + 360, 45) "; + EXPECT_EQ(d, Direction16::Degrees(135 - 360, 45)) + << "(135, 135) == (135 - 360, 45) "; + + d = Direction16::Degrees(0, 45 + 180); + EXPECT_EQ(d, Direction16::Degrees(180, -45)) << "(0, 45+180) == (180, -45)"; +} + +#endif \ No newline at end of file