Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add addition operator to Matrix6 #469

Merged
merged 3 commits into from
Jul 21, 2022
Merged

Add addition operator to Matrix6 #469

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
728c4a7
Add addition operator to Matrix6
chapulina Jul 20, 2022
d049d72
Suggestion to 469 (#470)
ahcorde Jul 21, 2022
a9add9e
Test coverage
chapulina Jul 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 43 additions & 0 deletions include/ignition/math/Matrix6.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,21 @@ namespace ignition
{
}

/// \brief Set a value in a specific row and col
/// param[in] _row Row of the matrix
/// param[in] _col Col of the matrix
/// param[in] _v Value to assign
/// \return Tru if the value was setted, False otherwise
public: bool SetValue(size_t _row, size_t _col, T _v)
{
if (_row < MatrixSize && _col < MatrixSize)
{
this->data[_row][_col] = _v;
return true;
}
return false;
}

/// \brief Change the values
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
Expand Down Expand Up @@ -317,6 +332,34 @@ namespace ignition
el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
}

/// \brief Addition assignment operator. This matrix will
/// become equal to this + _m2.
/// \param[in] _m2 Incoming matrix.
/// \return This matrix + _m2.
public: Matrix6<T> operator+=(const Matrix6<T> &_m2)
{
(*this) = (*this) + _m2;
return *this;
}

/// \brief Addition operator
/// \param[in] _m2 Incoming matrix
/// \return This matrix + _m2
public: Matrix6<T> operator+(const Matrix6<T> &_m2) const
{
auto el = [&](size_t _row, size_t _col) -> T
{
return this->data[_row][_col] + _m2(_row, _col);
};
return Matrix6<T>(
el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5),
el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5),
el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5),
el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5),
el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5),
el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5));
}

/// \brief Get the value at the specified row, column index
/// \param[in] _col The column index. Index values are clamped to a
/// range of [0, 5].
Expand Down
54 changes: 53 additions & 1 deletion src/Matrix6_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ TEST(Matrix6dTest, CoverageExtra)
}

/////////////////////////////////////////////////
TEST(Matrix6dTest, Multiply4)
TEST(Matrix6dTest, Multiply)
{
Matrix6d mat, mat1;

Expand Down Expand Up @@ -137,6 +137,36 @@ TEST(Matrix6dTest, Multiply4)
EXPECT_EQ(mat2, mat4);
}

/////////////////////////////////////////////////
TEST(Matrix6dTest, Add)
{
Matrix6d mat, mat1;

for (int i = 0; i < 6; ++i)
{
for (int j = 0; j < 6; ++j)
{
mat(i, j) = i - j;
mat1(j, i) = i + j;
}
}

Matrix6d mat3(
0, 0, 0, 0, 0, 0,
2, 2, 2, 2, 2, 2,
4, 4, 4, 4, 4, 4,
6, 6, 6, 6, 6, 6,
8, 8, 8, 8, 8, 8,
10, 10, 10, 10, 10, 10);

auto mat2 = mat + mat1;
EXPECT_EQ(mat2, mat3);

auto mat4 = mat;
mat4 += mat1;
EXPECT_EQ(mat2, mat4);
}

/////////////////////////////////////////////////
TEST(Matrix6dTest, NoIndexException)
{
Expand Down Expand Up @@ -341,3 +371,25 @@ TEST(Matrix6dTest, SetSubmatrix)
30, 31, 32, 33, 34, 35));
}

/////////////////////////////////////////////////
TEST(Matrix6dTest, SetValue)
{
Matrix6i mat;

for (int i = 0; i < 6; ++i)
{
for (int j = 0; j < 6; ++j)
{
EXPECT_TRUE(mat.SetValue(i, j, i - j));
}
}
EXPECT_FALSE(mat.SetValue(100, 100, 100));

EXPECT_EQ(mat, Matrix6i(
0, -1, -2, -3, -4, -5,
1, 0, -1, -2, -3, -4,
2, 1, 0, -1, -2, -3,
3, 2, 1, 0, -1, -2,
4, 3, 2, 1, 0, -1,
5, 4, 3, 2, 1, 0));
}
1 change: 1 addition & 0 deletions src/python_pybind11/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,7 @@ if (BUILD_TESTING)
Material_TEST
Matrix3_TEST
Matrix4_TEST
Matrix6_TEST
MovingWindowFilter_TEST
OrientedBox_TEST
PID_TEST
Expand Down
4 changes: 4 additions & 0 deletions src/python_pybind11/src/Matrix6.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr)
T, T, T, T, T, T,
T, T, T, T, T, T>())
.def(py::self * py::self)
.def(py::self + py::self)
.def(py::self == py::self)
.def(py::self != py::self)
.def("__call__",
Expand All @@ -76,6 +77,9 @@ void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr)
.def("set",
&Class::Set,
"Set values")
.def("set_value",
&Class::SetValue,
"Set value in a specific row and col.")
.def("equal",
&Class::Equal,
"Equality test operator")
Expand Down
124 changes: 74 additions & 50 deletions src/python_pybind11/test/Matrix6_TEST.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
# Copyright (C) 2022 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# Licensed under the Apache License, Version 2.0 (the "License")
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
Expand Down Expand Up @@ -38,48 +38,48 @@ def test_construct(self):
12.0, 13.0, 14.0, 15.0, 16.0, 17.0,
18.0, 19.0, 20.0, 21.0, 22.0, 23.0,
24.0, 25.0, 26.0, 27.0, 28.0, 29.0,
30.0, 31.0, 32.0, 33.0, 34.0, 35.0);
30.0, 31.0, 32.0, 33.0, 34.0, 35.0)

mat4 = Matrix6d(mat3)
self.assertAlmostEqual(mat4, mat3)

self.assertAlmostEqual(mat3(0, 0), 0.0);
self.assertAlmostEqual(mat3(0, 1), 1.0);
self.assertAlmostEqual(mat3(0, 2), 2.0);
self.assertAlmostEqual(mat3(0, 3), 3.0);
self.assertAlmostEqual(mat3(0, 4), 4.0);
self.assertAlmostEqual(mat3(0, 5), 5.0);
self.assertAlmostEqual(mat3(1, 0), 6.0);
self.assertAlmostEqual(mat3(1, 1), 7.0);
self.assertAlmostEqual(mat3(1, 2), 8.0);
self.assertAlmostEqual(mat3(1, 3), 9.0);
self.assertAlmostEqual(mat3(1, 4), 10.0);
self.assertAlmostEqual(mat3(1, 5), 11.0);
self.assertAlmostEqual(mat3(2, 0), 12.0);
self.assertAlmostEqual(mat3(2, 1), 13.0);
self.assertAlmostEqual(mat3(2, 2), 14.0);
self.assertAlmostEqual(mat3(2, 3), 15.0);
self.assertAlmostEqual(mat3(2, 4), 16.0);
self.assertAlmostEqual(mat3(2, 5), 17.0);
self.assertAlmostEqual(mat3(3, 0), 18.0);
self.assertAlmostEqual(mat3(3, 1), 19.0);
self.assertAlmostEqual(mat3(3, 2), 20.0);
self.assertAlmostEqual(mat3(3, 3), 21.0);
self.assertAlmostEqual(mat3(3, 4), 22.0);
self.assertAlmostEqual(mat3(3, 5), 23.0);
self.assertAlmostEqual(mat3(4, 0), 24.0);
self.assertAlmostEqual(mat3(4, 1), 25.0);
self.assertAlmostEqual(mat3(4, 2), 26.0);
self.assertAlmostEqual(mat3(4, 3), 27.0);
self.assertAlmostEqual(mat3(4, 4), 28.0);
self.assertAlmostEqual(mat3(4, 5), 29.0);
self.assertAlmostEqual(mat3(5, 0), 30.0);
self.assertAlmostEqual(mat3(5, 1), 31.0);
self.assertAlmostEqual(mat3(5, 2), 32.0);
self.assertAlmostEqual(mat3(5, 3), 33.0);
self.assertAlmostEqual(mat3(5, 4), 34.0);
self.assertAlmostEqual(mat3(5, 5), 35.0);
self.assertAlmostEqual(mat3(100, 100), 35.0);
self.assertAlmostEqual(mat3(0, 0), 0.0)
self.assertAlmostEqual(mat3(0, 1), 1.0)
self.assertAlmostEqual(mat3(0, 2), 2.0)
self.assertAlmostEqual(mat3(0, 3), 3.0)
self.assertAlmostEqual(mat3(0, 4), 4.0)
self.assertAlmostEqual(mat3(0, 5), 5.0)
self.assertAlmostEqual(mat3(1, 0), 6.0)
self.assertAlmostEqual(mat3(1, 1), 7.0)
self.assertAlmostEqual(mat3(1, 2), 8.0)
self.assertAlmostEqual(mat3(1, 3), 9.0)
self.assertAlmostEqual(mat3(1, 4), 10.0)
self.assertAlmostEqual(mat3(1, 5), 11.0)
self.assertAlmostEqual(mat3(2, 0), 12.0)
self.assertAlmostEqual(mat3(2, 1), 13.0)
self.assertAlmostEqual(mat3(2, 2), 14.0)
self.assertAlmostEqual(mat3(2, 3), 15.0)
self.assertAlmostEqual(mat3(2, 4), 16.0)
self.assertAlmostEqual(mat3(2, 5), 17.0)
self.assertAlmostEqual(mat3(3, 0), 18.0)
self.assertAlmostEqual(mat3(3, 1), 19.0)
self.assertAlmostEqual(mat3(3, 2), 20.0)
self.assertAlmostEqual(mat3(3, 3), 21.0)
self.assertAlmostEqual(mat3(3, 4), 22.0)
self.assertAlmostEqual(mat3(3, 5), 23.0)
self.assertAlmostEqual(mat3(4, 0), 24.0)
self.assertAlmostEqual(mat3(4, 1), 25.0)
self.assertAlmostEqual(mat3(4, 2), 26.0)
self.assertAlmostEqual(mat3(4, 3), 27.0)
self.assertAlmostEqual(mat3(4, 4), 28.0)
self.assertAlmostEqual(mat3(4, 5), 29.0)
self.assertAlmostEqual(mat3(5, 0), 30.0)
self.assertAlmostEqual(mat3(5, 1), 31.0)
self.assertAlmostEqual(mat3(5, 2), 32.0)
self.assertAlmostEqual(mat3(5, 3), 33.0)
self.assertAlmostEqual(mat3(5, 4), 34.0)
self.assertAlmostEqual(mat3(5, 5), 35.0)
self.assertAlmostEqual(mat3(100, 100), 35.0)

def test_multiply_mat(self):
mat = Matrix6d(0, -1, -2, -3, -4, -5,
Expand Down Expand Up @@ -111,14 +111,38 @@ def test_multiply_mat(self):
mat4 *= mat1
self.assertAlmostEqual(mat2, mat4)

def test_add_mat(self):
mat = Matrix6d()
mat1 = Matrix6d()

for i in range(6):
for j in range(6):
mat.set_value(i, j, i - j)
mat1.set_value(j, i, i + j)

mat3 = Matrix6d(
0, 0, 0, 0, 0, 0,
2, 2, 2, 2, 2, 2,
4, 4, 4, 4, 4, 4,
6, 6, 6, 6, 6, 6,
8, 8, 8, 8, 8, 8,
10, 10, 10, 10, 10, 10)

mat2 = mat + mat1
self.assertAlmostEqual(mat2, mat3)

mat4 = mat
mat4 += mat1
self.assertAlmostEqual(mat2, mat4)

def test_stream_out(self):
matA = Matrix6d(
0.0, 1.0, 2.0, 3.0, 4.0, 5.0,
6.0, 7.0, 8.0, 9.0, 10.0, 11.0,
12.0, 13.0, 14.0, 15.0, 16.0, 17.0,
18.0, 19.0, 20.0, 21.0, 22.0, 23.0,
24.0, 25.0, 26.0, 27.0, 28.0, 29.0,
30.0, 31.0, 32.0, 33.0, 34.0, 35.0);
30.0, 31.0, 32.0, 33.0, 34.0, 35.0)

self.assertEqual(str(matA),
"0 1 2 3 4 5 "
Expand Down Expand Up @@ -177,58 +201,58 @@ def test_submatrix(self):
12, 13, 14, 15, 16, 17,
18, 19, 20, 21, 22, 23,
24, 25, 26, 27, 28, 29,
30, 31, 32, 33, 34, 35);
30, 31, 32, 33, 34, 35)

self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_LEFT), Matrix3d(
0, 1, 2,
6, 7, 8,
12, 13, 14));
12, 13, 14))

self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_RIGHT), Matrix3d(
3, 4, 5,
9, 10, 11,
15, 16, 17));
15, 16, 17))

self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_LEFT), Matrix3d(
18, 19, 20,
24, 25, 26,
30, 31, 32));
30, 31, 32))

self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_RIGHT), Matrix3d(
21, 22, 23,
27, 28, 29,
33, 34, 35));
33, 34, 35))

def test_set_submatrix(self):
mat = Matrix6d()

mat.set_submatrix(Matrix6dCorner.TOP_LEFT, Matrix3d(
0, 1, 2,
6, 7, 8,
12, 13, 14));
12, 13, 14))

mat.set_submatrix(Matrix6dCorner.TOP_RIGHT, Matrix3d(
3, 4, 5,
9, 10, 11,
15, 16, 17));
15, 16, 17))

mat.set_submatrix(Matrix6dCorner.BOTTOM_LEFT, Matrix3d(
18, 19, 20,
24, 25, 26,
30, 31, 32));
30, 31, 32))

mat.set_submatrix(Matrix6dCorner.BOTTOM_RIGHT, Matrix3d(
21, 22, 23,
27, 28, 29,
33, 34, 35));
33, 34, 35))

self.assertAlmostEqual(mat, Matrix6d(
0, 1, 2, 3, 4, 5,
6, 7, 8, 9, 10, 11,
12, 13, 14, 15, 16, 17,
18, 19, 20, 21, 22, 23,
24, 25, 26, 27, 28, 29,
30, 31, 32, 33, 34, 35));
30, 31, 32, 33, 34, 35))

if __name__ == '__main__':
unittest.main()