From 5c9fac79ae6a49da6e087f408465c71d9d82d3cb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 3 Mar 2023 19:02:47 +0100 Subject: [PATCH] Vector2: Add function to rotate a 2D vector --- src/lib/matrix/matrix/Vector2.hpp | 25 +++++++++++++++++++++++ src/lib/matrix/test/MatrixVector2Test.cpp | 16 +++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 35ceb7c1ac..e3284feb11 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -71,6 +71,31 @@ public: return (*this).cross(b); } + /** + * @brief rotate vector + * + * @param[in] angle [rad] The angle around the positive out of plane axis. + */ + void rotate(const Type angle) + { + Vector2 &v(*this); + Type sin_angle = sin(angle); + Type cos_angle = cos(angle); + v = Vector2( + cos_angle * v(0) - sin_angle * v(1), + sin_angle * v(0) + cos_angle * v(1) + ); + } + + /** + * @brief Transform vector to new coordinate system + * + * @param[in] angle [rad] The rotation of new coordinate system around the positive out of plane axis. + */ + void transform(const Type angle) + { + this->rotate(-angle); + } }; diff --git a/src/lib/matrix/test/MatrixVector2Test.cpp b/src/lib/matrix/test/MatrixVector2Test.cpp index 23effd262b..3a0cb9bbaf 100644 --- a/src/lib/matrix/test/MatrixVector2Test.cpp +++ b/src/lib/matrix/test/MatrixVector2Test.cpp @@ -64,4 +64,20 @@ TEST(MatrixVector2Test, Vector2) Vector2f h(g); EXPECT_FLOAT_EQ(h(0), 1.23f); EXPECT_FLOAT_EQ(h(1), 423.4f); + + // rotate 2D vector + Vector2f i(1.0f, 0.0f); + i.rotate(M_PI_PRECISE / 4.f); + EXPECT_FLOAT_EQ(i(0), i(1)); + i.rotate(M_PI_PRECISE / 4.f); + EXPECT_FLOAT_EQ(i(0), 0.f); + EXPECT_FLOAT_EQ(i(1), 1.f); + + // transform vector + Vector2f j(1.0f, 0.0f); + j.transform(-M_PI_PRECISE / 4.f); + EXPECT_FLOAT_EQ(j(0), j(1)); + j.transform(-M_PI_PRECISE / 4.f); + EXPECT_FLOAT_EQ(j(0), 0.f); + EXPECT_FLOAT_EQ(j(1), 1.f); }