Skip to content

Commit

Permalink
Add addition operator and SetValue to Matrix6 (#469)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <[email protected]>
Signed-off-by: ahcorde <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@chapulina @ahcorde
chapulina and ahcorde authored Jul 21, 2022
1 parent 88ee511 commit 6fbc966
Show file tree
Hide file tree
Showing 5 changed files with 175 additions and 51 deletions.
43 changes: 43 additions & 0 deletions include/ignition/math/Matrix6.hh
View file
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
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
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
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
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()

0 comments on commit 6fbc966

Please sign in to comment.