From 37d3218d113783d9967a5bd837ceb2b3c7574ce6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 7 Mar 2023 12:00:14 +0100 Subject: [PATCH] matrix: add 2d rotation matrix --- src/lib/matrix/matrix/Dcm2.hpp | 127 +++++++++++++++++++++++++ src/lib/matrix/matrix/math.hpp | 1 + src/lib/matrix/test/CMakeLists.txt | 1 + src/lib/matrix/test/MatrixDcm2Test.cpp | 75 +++++++++++++++ 4 files changed, 204 insertions(+) create mode 100644 src/lib/matrix/matrix/Dcm2.hpp create mode 100644 src/lib/matrix/test/MatrixDcm2Test.cpp diff --git a/src/lib/matrix/matrix/Dcm2.hpp b/src/lib/matrix/matrix/Dcm2.hpp new file mode 100644 index 0000000000..ae3b12c1f4 --- /dev/null +++ b/src/lib/matrix/matrix/Dcm2.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Dcm2.hpp + * + * A givens rotation matrix 2x2. + * All rotations and axis systems follow the right-hand rule. + * + * This library uses the convention that premultiplying a two dimensional + * vector represented in coordinate system 1 will apply a rotation from coordinate system + * 1 to coordinate system 2 to the vector. + * Likewise, a matrix instance of this class also represents a coordinate transformation + * from frame 2 to frame 1. + * + * @author Matthias Grob + */ + +#pragma once + +#include "math.hpp" + +namespace matrix +{ + +template +class Dcm2 : public SquareMatrix +{ +public: + /** + * Standard constructor + * + * Initializes to identity + */ + Dcm2() : SquareMatrix(eye()) {} + + /** + * Constructor from array + * + * @param _data pointer to array + */ + explicit Dcm2(const Type data_[2][2]) : SquareMatrix(data_) + { + } + + /** + * Constructor from array + * + * @param _data pointer to array + */ + explicit Dcm2(const Type data_[4]) : SquareMatrix(data_) + { + } + + /** + * Copy constructor + * + * @param other Matrix22 to set dcm to + */ + Dcm2(const Matrix &other) : SquareMatrix(other) + { + } + + /** + * Constructor from an angle + * + * This sets the transformation matrix from frame 2 to frame 1 where the rotation + * from frame 1 to frame 2 is described by an angle in radians. + * + * @param angle from frame 1 to frame 2 in radians + */ + Dcm2(const Type angle) + { + Dcm2 &dcm = *this; + Type sin_angle = sin(angle); + Type cos_angle = cos(angle); + + dcm(0, 0) = cos_angle; + dcm(0, 1) = -sin_angle; + dcm(1, 0) = sin_angle; + dcm(1, 1) = cos_angle; + } + + void renormalize() + { + /* renormalize rows */ + for (size_t r = 0; r < 2; r++) { + matrix::Vector2 rvec(Matrix(this->Matrix::row(r)).transpose()); + this->Matrix::row(r) = rvec.normalized(); + } + } +}; + +using Dcm2f = Dcm2; +using Dcm2d = Dcm2; + +} // namespace matrix diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index 9e2d071f92..dd80723d85 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -14,6 +14,7 @@ #include "Vector4.hpp" #include "Euler.hpp" #include "Dcm.hpp" +#include "Dcm2.hpp" #include "Scalar.hpp" #include "Quaternion.hpp" #include "AxisAngle.hpp" diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt index c3ea1d8e6c..dbdf1d4c22 100644 --- a/src/lib/matrix/test/CMakeLists.txt +++ b/src/lib/matrix/test/CMakeLists.txt @@ -6,6 +6,7 @@ add_compile_options( px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) px4_add_unit_gtest(SRC MatrixAttitudeTest.cpp) px4_add_unit_gtest(SRC MatrixCopyToTest.cpp) +px4_add_unit_gtest(SRC MatrixDcm2Test.cpp) px4_add_unit_gtest(SRC MatrixDualTest.cpp) px4_add_unit_gtest(SRC MatrixFilterTest.cpp) px4_add_unit_gtest(SRC MatrixHatveeTest.cpp) diff --git a/src/lib/matrix/test/MatrixDcm2Test.cpp b/src/lib/matrix/test/MatrixDcm2Test.cpp new file mode 100644 index 0000000000..32cdc36085 --- /dev/null +++ b/src/lib/matrix/test/MatrixDcm2Test.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +using namespace matrix; + +TEST(MatrixDcm2Test, CompareToDcm) +{ + // 2D matrix and 3D matrix rotating around yaw are equivalent + Dcm2f R2(1.f); + Dcmf R3(Eulerf(0.f, 0.f, 1.f)); + EXPECT_EQ(Dcm2f(R3.slice<2, 2>(0, 0)), R2); + + // Rotating a 2D and a 3D vector respectively is equivalent + Vector2f v2(1.f, 2.f); + Vector3f v3(v2(0), v2(1), 0.f); + Vector2f result2 = R2 * v2; + Vector3f result3 = R3 * v3; + EXPECT_EQ(Vector2f(result2), result2); + EXPECT_EQ(result2, result3.xy()); +} + +TEST(MatrixDcm2Test, 45DegreeRotation) +{ + Vector2f v(1.0f, 0.0f); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(1.f, 1.f) * (sqrt(2) / 2)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(0.f, 1.f)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(-1.f, 1.f) * (sqrt(2) / 2)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(-1.f, 0.f)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(-1.f, -1.f) * (sqrt(2) / 2)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(0.f, -1.f)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(1.f, -1.f) * (sqrt(2) / 2)); + v = Dcm2f(M_PI / 4.f) * v; + EXPECT_EQ(v, Vector2f(1.f, 0.f)); +}