Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-18 00:08:22 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

Misc: Matrix: Added Addition and Subtraction to Slices (#23679)

Browse Source 
* Added Addition and Subtraction to Slices

* MatrixSliceTest: refactor Addition/Substraction checks

* Slice: replace operations returning a Matrix with calling the existing Matrix function

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Claudio Chies
2024-09-18 11:17:37 +02:00
committed by GitHub
parent 4a99a51fb1
commit 5d2e7c8748
5 changed files with 122 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Slice.hpp
+39 -16
View File
@@ -116,6 +116,39 @@ public:
return self;
}
template<size_t MM, size_t NN>
Matrix<Type, P, Q> operator-(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
{
return Matrix<Type, P, Q> {*this} - other;
}
Matrix<Type, P, Q> operator-(const Matrix<Type, P, Q> &other)
{
return Matrix<Type, P, Q> {*this} - other;
}
Matrix<Type, P, Q> operator-(const Type &other)
{
return Matrix<Type, P, Q> {*this} - other;
}
template<size_t MM, size_t NN>
Matrix<Type, P, Q> operator+(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
{
return Matrix<Type, P, Q> {*this} + other;
}
Matrix<Type, P, Q> operator+(const Matrix<Type, P, Q> &other)
{
return Matrix<Type, P, Q> {*this} + other;
}
Matrix<Type, P, Q> operator+(const Type &other)
{
return Matrix<Type, P, Q> {*this} + other;
}
// allow assigning vectors to a slice that are in the axis
template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
@@ -222,29 +255,19 @@ public:
return self;
}
SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
{
return operator*=(Type(1) / other);
return operator*=(Type(1) / scalar);
}
Matrix<Type, P, Q> operator*(const Type &other) const
Matrix<Type, P, Q> operator*(const Type &scalar) const
{
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
Matrix<Type, P, Q> res;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
res(i, j) = self(i, j) * other;
}
}
return res;
return Matrix<Type, P, Q> {*this} * scalar;
}
Matrix<Type, P, Q> operator/(const Type &other) const
Matrix<Type, P, Q> operator/(const Type &scalar) const
{
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
return self * (Type(1) / other);
return (*this) * (1 / scalar);
}
template<size_t R, size_t S>
src/lib/matrix/matrix/SquareMatrix.hpp
+1
View File
@@ -317,6 +317,7 @@ public:
}
};
using SquareMatrix2f = SquareMatrix<float, 2>;
using SquareMatrix3f = SquareMatrix<float, 3>;
using SquareMatrix3d = SquareMatrix<double, 3>;
src/lib/matrix/matrix/Vector2.hpp
-1
View File
@@ -102,7 +102,6 @@ public:
};
using Vector2f = Vector2<float>;
using Vector2d = Vector2<double>;
src/lib/matrix/test/MatrixSliceTest.cpp
+73
View File
@@ -262,6 +262,79 @@ TEST(MatrixSliceTest, Slice)
float O_check_data_12 [4] = {2.5, 3, 4, 5};
EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
}
TEST(MatrixSliceTest, SliceAdditions)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix3f A{data};
float operand_data [4] = {2, 1,
-3, -1
};
const SquareMatrix2f operand(operand_data);
// 2x2 Slice + 2x2 Matrix
SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand;
float res_1_check_data[4] = {6, 6,
4, 7
};
EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
// 2x1 Slice + 2x1 Slice
Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0);
EXPECT_EQ(res_2, Vector2f(7, 5));
// 3x3 Slice + Scalar
SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1);
float res_3_check_data[9] = {-1, 1, 2,
3, 4, 5,
6, 7, 9
};
EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
// 3x1 Slice + 3 Vector
Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3);
EXPECT_EQ(res_4, Vector3f(3, 3, 11));
}
TEST(MatrixSliceTest, SliceSubtractions)
{
float data[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
SquareMatrix3f A{data};
float operand_data[4] = {2, 1,
-3, -1
};
const SquareMatrix2f operand(operand_data);
// 2x2 Slice - 2x2 Matrix
SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand;
float res_1_check_data[4] = {2, 4,
10, 9
};
EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
// 2x1 Slice - 2x1 Slice
Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0);
EXPECT_EQ(res_2, Vector2f(3, 11));
// 3x3 Slice - Scalar
SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1);
float res_3_check_data[9] = {1, 3, 4,
5, 6, 7,
8, 9, 11
};
EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
// 3x1 Slice - 3 Vector
Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3);
EXPECT_EQ(res_4, Vector3f(1, 7, 5));
}
TEST(MatrixSliceTest, XYAssignmentTest)
{
src/lib/matrix/test/MatrixVector3Test.cpp
+9
View File
@@ -80,4 +80,13 @@ TEST(MatrixVector3Test, Vector3)
Vector3f m2(3.1f, 4.1f, 5.1f);
EXPECT_EQ(m2, m1 + 2.1f);
EXPECT_EQ(m2 - 2.1f, m1);
// Test Addition and Subtraction of Slices
Vector3f v1(3, 13, 0);
Vector3f v2(42, 6, 256);
EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7));
EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19));
EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15));
EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11));
}