From 5c43624b3942f408f075f438d7c5c399851c0f6c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Jul 2022 19:38:36 -0700 Subject: [PATCH 01/10] Backport newest appendToStream functions Signed-off-by: Louise Poubel --- include/ignition/math/Helpers.hh | 25 +++++++++++++++ src/Helpers_TEST.cc | 52 ++++++++++++++++++++++++++++++++ 2 files changed, 77 insertions(+) diff --git a/include/ignition/math/Helpers.hh b/include/ignition/math/Helpers.hh index 7152b5228..5d69b209d 100644 --- a/include/ignition/math/Helpers.hh +++ b/include/ignition/math/Helpers.hh @@ -619,6 +619,31 @@ namespace ignition sort2(_a, _b); } + /// \brief Append a number to a stream. Makes sure "-0" is returned as "0". + /// \param[out] _out Output stream. + /// \param[in] _number Number to append. + template + inline void appendToStream(std::ostream &_out, T _number) + { + if (std::fpclassify(_number) == FP_ZERO) + { + _out << 0; + } + else + { + _out << _number; + } + } + + /// \brief Append a number to a stream, specialized for int. + /// \param[out] _out Output stream. + /// \param[in] _number Number to append. + template<> + inline void appendToStream(std::ostream &_out, int _number) + { + _out << _number; + } + /// \brief Is this a power of 2? /// \param[in] _x the number /// \return true if _x is a power of 2, false otherwise diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc index 32bde4df7..a602f210d 100644 --- a/src/Helpers_TEST.cc +++ b/src/Helpers_TEST.cc @@ -970,3 +970,55 @@ TEST(HelpersTest, roundUpMultiple) EXPECT_EQ(0, math::roundUpMultiple(0, -2)); EXPECT_EQ(-2, math::roundUpMultiple(-2, -2)); } + +///////////////////////////////////////////////// +TEST(HelpersTest, AppendToStream) +{ + std::ostringstream out; + + math::appendToStream(out, 0.0f); + EXPECT_EQ(out.str(), "0"); + + out << " "; + + math::appendToStream(out, 456); + EXPECT_EQ(out.str(), "0 456"); + + out << " "; + + math::appendToStream(out, 0); + EXPECT_EQ(out.str(), "0 456 0"); + + out << " "; + + // ref: https://en.cppreference.com/w/cpp/io/manip/setprecision + const long double pi = std::acos(-1.L); + math::appendToStream(out, pi); + EXPECT_EQ(out.str(), "0 456 0 3.14159"); + + out << " " + << std::setprecision(10); + + math::appendToStream(out, pi); + EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654"); + + out << " " + << std::setprecision(std::numeric_limits::digits10 + 1); + + math::appendToStream(out, pi); +#ifdef _WIN32 + EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793"); +#else + EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793239"); +#endif + + out << " " + << std::setprecision(3); + + math::appendToStream(out, pi); +#ifdef _WIN32 + EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793 3.14"); +#else + EXPECT_EQ(out.str(), "0 456 0 3.14159 3.141592654 3.141592653589793239 3.14"); +#endif +} From e7668ec5a898820b6207bff6a18627d41c9dab3f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Jul 2022 19:31:58 -0700 Subject: [PATCH 02/10] Port main functionality from Matrix4 to Matrix6, with updates from gz-math7 Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 663 +++++++++++++++++++++++++++++++ src/Matrix6_TEST.cc | 277 +++++++++++++ 2 files changed, 940 insertions(+) create mode 100644 include/ignition/math/Matrix6.hh create mode 100644 src/Matrix6_TEST.cc diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh new file mode 100644 index 000000000..2027c1584 --- /dev/null +++ b/include/ignition/math/Matrix6.hh @@ -0,0 +1,663 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_MATH_MATRIX6_HH_ +#define GZ_MATH_MATRIX6_HH_ + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Matrix6 Matrix6.hh ignition/math/Matrix6.hh + /// \brief A 6x6 matrix class + template + class Matrix6 + { + /// \brief Size of matrix is fixed to 6x6 + public: static constexpr std::size_t MatrixSize{6}; + + /// \brief Identity matrix + public: static const Matrix6 &Identity; + + /// \brief Zero matrix + public: static const Matrix6 &Zero; + + /// \brief Constructor + public: Matrix6() + { + memset(this->data, 0, sizeof(this->data[0][0])*MatrixSize*MatrixSize); + } + + /// \brief Copy constructor + /// \param _m Matrix to copy + public: Matrix6(const Matrix6 &_m) = default; + + /// \brief Constructor + /// \param[in] _v00 Row 0, Col 0 value + /// \param[in] _v01 Row 0, Col 1 value + /// \param[in] _v02 Row 0, Col 2 value + /// \param[in] _v03 Row 0, Col 3 value + /// \param[in] _v04 Row 0, Col 4 value + /// \param[in] _v05 Row 0, Col 5 value + /// \param[in] _v10 Row 1, Col 0 value + /// \param[in] _v11 Row 1, Col 1 value + /// \param[in] _v12 Row 1, Col 2 value + /// \param[in] _v13 Row 1, Col 3 value + /// \param[in] _v14 Row 1, Col 4 value + /// \param[in] _v15 Row 1, Col 5 value + /// \param[in] _v20 Row 2, Col 0 value + /// \param[in] _v21 Row 2, Col 1 value + /// \param[in] _v22 Row 2, Col 2 value + /// \param[in] _v23 Row 2, Col 3 value + /// \param[in] _v24 Row 2, Col 4 value + /// \param[in] _v25 Row 2, Col 5 value + /// \param[in] _v30 Row 3, Col 0 value + /// \param[in] _v31 Row 3, Col 1 value + /// \param[in] _v32 Row 3, Col 2 value + /// \param[in] _v33 Row 3, Col 3 value + /// \param[in] _v34 Row 3, Col 4 value + /// \param[in] _v35 Row 3, Col 5 value + /// \param[in] _v40 Row 4, Col 0 value + /// \param[in] _v41 Row 4, Col 1 value + /// \param[in] _v42 Row 4, Col 2 value + /// \param[in] _v43 Row 4, Col 3 value + /// \param[in] _v44 Row 4, Col 4 value + /// \param[in] _v45 Row 4, Col 5 value + /// \param[in] _v50 Row 5, Col 0 value + /// \param[in] _v51 Row 5, Col 1 value + /// \param[in] _v52 Row 5, Col 2 value + /// \param[in] _v53 Row 5, Col 3 value + /// \param[in] _v54 Row 5, Col 4 value + /// \param[in] _v55 Row 5, Col 5 value + public: constexpr Matrix6(T _v00, T _v01, T _v02, T _v03, T _v04, T _v05, + T _v10, T _v11, T _v12, T _v13, T _v14, T _v15, + T _v20, T _v21, T _v22, T _v23, T _v24, T _v25, + T _v30, T _v31, T _v32, T _v33, T _v34, T _v35, + T _v40, T _v41, T _v42, T _v43, T _v44, T _v45, + T _v50, T _v51, T _v52, T _v53, T _v54, T _v55) + : data{{_v00, _v01, _v02, _v03, _v04, _v05}, + {_v10, _v11, _v12, _v13, _v14, _v15}, + {_v20, _v21, _v22, _v23, _v24, _v25}, + {_v30, _v31, _v32, _v33, _v34, _v35}, + {_v40, _v41, _v42, _v43, _v44, _v45}, + {_v50, _v51, _v52, _v53, _v54, _v55}} + { + } + + /// \brief Change the values + /// \param[in] _v00 Row 0, Col 0 value + /// \param[in] _v01 Row 0, Col 1 value + /// \param[in] _v02 Row 0, Col 2 value + /// \param[in] _v03 Row 0, Col 3 value + /// \param[in] _v10 Row 1, Col 0 value + /// \param[in] _v11 Row 1, Col 1 value + /// \param[in] _v12 Row 1, Col 2 value + /// \param[in] _v13 Row 1, Col 3 value + /// \param[in] _v20 Row 2, Col 0 value + /// \param[in] _v21 Row 2, Col 1 value + /// \param[in] _v22 Row 2, Col 2 value + /// \param[in] _v23 Row 2, Col 3 value + /// \param[in] _v30 Row 3, Col 0 value + /// \param[in] _v31 Row 3, Col 1 value + /// \param[in] _v32 Row 3, Col 2 value + /// \param[in] _v33 Row 3, Col 3 value + public: void Set( + T _v00, T _v01, T _v02, T _v03, T _v04, T _v05, + T _v10, T _v11, T _v12, T _v13, T _v14, T _v15, + T _v20, T _v21, T _v22, T _v23, T _v24, T _v25, + T _v30, T _v31, T _v32, T _v33, T _v34, T _v35, + T _v40, T _v41, T _v42, T _v43, T _v44, T _v45, + T _v50, T _v51, T _v52, T _v53, T _v54, T _v55) + { + this->data[0][0] = _v00; + this->data[0][1] = _v01; + this->data[0][2] = _v02; + this->data[0][3] = _v03; + this->data[0][4] = _v04; + this->data[0][5] = _v05; + + this->data[1][0] = _v10; + this->data[1][1] = _v11; + this->data[1][2] = _v12; + this->data[1][3] = _v13; + this->data[1][4] = _v14; + this->data[1][5] = _v15; + + this->data[2][0] = _v20; + this->data[2][1] = _v21; + this->data[2][2] = _v22; + this->data[2][3] = _v23; + this->data[2][4] = _v24; + this->data[2][5] = _v25; + + this->data[3][0] = _v30; + this->data[3][1] = _v31; + this->data[3][2] = _v32; + this->data[3][3] = _v33; + this->data[3][4] = _v34; + this->data[3][5] = _v35; + + this->data[4][0] = _v40; + this->data[4][1] = _v41; + this->data[4][2] = _v42; + this->data[4][3] = _v43; + this->data[4][4] = _v44; + this->data[4][5] = _v45; + + this->data[5][0] = _v50; + this->data[5][1] = _v51; + this->data[5][2] = _v52; + this->data[5][3] = _v53; + this->data[5][4] = _v54; + this->data[5][5] = _v55; + } + + /// \brief Transpose this matrix. + public: void Transpose() + { + std::swap(this->data[0][1], this->data[1][0]); + std::swap(this->data[0][2], this->data[2][0]); + std::swap(this->data[0][3], this->data[3][0]); + std::swap(this->data[0][4], this->data[4][0]); + std::swap(this->data[0][5], this->data[5][0]); + std::swap(this->data[1][2], this->data[2][1]); + std::swap(this->data[1][3], this->data[3][1]); + std::swap(this->data[1][4], this->data[4][1]); + std::swap(this->data[1][5], this->data[5][1]); + std::swap(this->data[2][3], this->data[3][2]); + std::swap(this->data[2][4], this->data[4][2]); + std::swap(this->data[2][5], this->data[5][2]); + std::swap(this->data[3][4], this->data[4][3]); + std::swap(this->data[3][5], this->data[5][3]); + std::swap(this->data[4][5], this->data[5][4]); + } + + /// \brief Return the transpose of this matrix + /// \return Transpose of this matrix. + public: Matrix6 Transposed() const + { + return Matrix6( + this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0], + this->data[4][0], this->data[5][0], + this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1], + this->data[4][1], this->data[5][1], + this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2], + this->data[4][2], this->data[5][2], + this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3], + this->data[4][3], this->data[5][3], + this->data[0][4], this->data[1][4], this->data[2][4], this->data[3][4], + this->data[4][4], this->data[5][4], + this->data[0][5], this->data[1][5], this->data[2][5], this->data[3][5], + this->data[4][5], this->data[5][5]); + } + + /// \brief Assignment operator. this = _mat + /// \param _mat Incoming matrix + /// \return itself + public: Matrix6 &operator=(const Matrix6 &_mat) = default; + + /// \brief Multiplication assignment operator. This matrix will + /// become equal to this * _m2. + /// \param[in] _m2 Incoming matrix. + /// \return This matrix * _mat. + public: Matrix6 operator*=(const Matrix6 &_m2) + { + (*this) = (*this) * _m2; + return *this; + } + + /// \brief Multiplication operator + /// \param[in] _m2 Incoming matrix + /// \return This matrix * _mat + public: Matrix6 operator*(const Matrix6 &_m2) const + { + return Matrix6( + this->data[0][0] * _m2(0, 0) + + this->data[0][1] * _m2(1, 0) + + this->data[0][2] * _m2(2, 0) + + this->data[0][3] * _m2(3, 0) + + this->data[0][4] * _m2(4, 0) + + this->data[0][5] * _m2(5, 0), + + this->data[0][0] * _m2(0, 1) + + this->data[0][1] * _m2(1, 1) + + this->data[0][2] * _m2(2, 1) + + this->data[0][3] * _m2(3, 1) + + this->data[0][4] * _m2(4, 1) + + this->data[0][5] * _m2(5, 1), + + this->data[0][0] * _m2(0, 2) + + this->data[0][1] * _m2(1, 2) + + this->data[0][2] * _m2(2, 2) + + this->data[0][3] * _m2(3, 2) + + this->data[0][4] * _m2(4, 2) + + this->data[0][5] * _m2(5, 2), + + this->data[0][0] * _m2(0, 3) + + this->data[0][1] * _m2(1, 3) + + this->data[0][2] * _m2(2, 3) + + this->data[0][3] * _m2(3, 3) + + this->data[0][4] * _m2(4, 3) + + this->data[0][5] * _m2(5, 3), + + this->data[0][0] * _m2(0, 4) + + this->data[0][1] * _m2(1, 4) + + this->data[0][2] * _m2(2, 4) + + this->data[0][3] * _m2(3, 4) + + this->data[0][4] * _m2(4, 4) + + this->data[0][5] * _m2(5, 4), + + this->data[0][0] * _m2(0, 5) + + this->data[0][1] * _m2(1, 5) + + this->data[0][2] * _m2(2, 5) + + this->data[0][3] * _m2(3, 5) + + this->data[0][4] * _m2(4, 5) + + this->data[0][5] * _m2(5, 5), + + this->data[1][0] * _m2(0, 0) + + this->data[1][1] * _m2(1, 0) + + this->data[1][2] * _m2(2, 0) + + this->data[1][3] * _m2(3, 0) + + this->data[1][4] * _m2(4, 0) + + this->data[1][5] * _m2(5, 0), + + this->data[1][0] * _m2(0, 1) + + this->data[1][1] * _m2(1, 1) + + this->data[1][2] * _m2(2, 1) + + this->data[1][3] * _m2(3, 1) + + this->data[1][4] * _m2(4, 1) + + this->data[1][5] * _m2(5, 1), + + this->data[1][0] * _m2(0, 2) + + this->data[1][1] * _m2(1, 2) + + this->data[1][2] * _m2(2, 2) + + this->data[1][3] * _m2(3, 2) + + this->data[1][4] * _m2(4, 2) + + this->data[1][5] * _m2(5, 2), + + this->data[1][0] * _m2(0, 3) + + this->data[1][1] * _m2(1, 3) + + this->data[1][2] * _m2(2, 3) + + this->data[1][3] * _m2(3, 3) + + this->data[1][4] * _m2(4, 3) + + this->data[1][5] * _m2(5, 3), + + this->data[1][0] * _m2(0, 4) + + this->data[1][1] * _m2(1, 4) + + this->data[1][2] * _m2(2, 4) + + this->data[1][3] * _m2(3, 4) + + this->data[1][4] * _m2(4, 4) + + this->data[1][5] * _m2(5, 4), + + this->data[1][0] * _m2(0, 5) + + this->data[1][1] * _m2(1, 5) + + this->data[1][2] * _m2(2, 5) + + this->data[1][3] * _m2(3, 5) + + this->data[1][4] * _m2(4, 5) + + this->data[1][5] * _m2(5, 5), + + this->data[2][0] * _m2(0, 0) + + this->data[2][1] * _m2(1, 0) + + this->data[2][2] * _m2(2, 0) + + this->data[2][3] * _m2(3, 0) + + this->data[2][4] * _m2(4, 0) + + this->data[2][5] * _m2(5, 0), + + this->data[2][0] * _m2(0, 1) + + this->data[2][1] * _m2(1, 1) + + this->data[2][2] * _m2(2, 1) + + this->data[2][3] * _m2(3, 1) + + this->data[2][4] * _m2(4, 1) + + this->data[2][5] * _m2(5, 1), + + this->data[2][0] * _m2(0, 2) + + this->data[2][1] * _m2(1, 2) + + this->data[2][2] * _m2(2, 2) + + this->data[2][3] * _m2(3, 2) + + this->data[2][4] * _m2(4, 2) + + this->data[2][5] * _m2(5, 2), + + this->data[2][0] * _m2(0, 3) + + this->data[2][1] * _m2(1, 3) + + this->data[2][2] * _m2(2, 3) + + this->data[2][3] * _m2(3, 3) + + this->data[2][4] * _m2(4, 3) + + this->data[2][5] * _m2(5, 3), + + this->data[2][0] * _m2(0, 4) + + this->data[2][1] * _m2(1, 4) + + this->data[2][2] * _m2(2, 4) + + this->data[2][3] * _m2(3, 4) + + this->data[2][4] * _m2(4, 4) + + this->data[2][5] * _m2(5, 4), + + this->data[2][0] * _m2(0, 5) + + this->data[2][1] * _m2(1, 5) + + this->data[2][2] * _m2(2, 5) + + this->data[2][3] * _m2(3, 5) + + this->data[2][4] * _m2(4, 5) + + this->data[2][5] * _m2(5, 5), + + this->data[3][0] * _m2(0, 0) + + this->data[3][1] * _m2(1, 0) + + this->data[3][2] * _m2(2, 0) + + this->data[3][3] * _m2(3, 0) + + this->data[3][4] * _m2(4, 0) + + this->data[3][5] * _m2(5, 0), + + this->data[3][0] * _m2(0, 1) + + this->data[3][1] * _m2(1, 1) + + this->data[3][2] * _m2(2, 1) + + this->data[3][3] * _m2(3, 1) + + this->data[3][4] * _m2(4, 1) + + this->data[3][5] * _m2(5, 1), + + this->data[3][0] * _m2(0, 2) + + this->data[3][1] * _m2(1, 2) + + this->data[3][2] * _m2(2, 2) + + this->data[3][3] * _m2(3, 2) + + this->data[3][4] * _m2(4, 2) + + this->data[3][5] * _m2(5, 2), + + this->data[3][0] * _m2(0, 3) + + this->data[3][1] * _m2(1, 3) + + this->data[3][2] * _m2(2, 3) + + this->data[3][3] * _m2(3, 3) + + this->data[3][4] * _m2(4, 3) + + this->data[3][5] * _m2(5, 3), + + this->data[3][0] * _m2(0, 4) + + this->data[3][1] * _m2(1, 4) + + this->data[3][2] * _m2(2, 4) + + this->data[3][3] * _m2(3, 4) + + this->data[3][4] * _m2(4, 4) + + this->data[3][5] * _m2(5, 4), + + this->data[3][0] * _m2(0, 5) + + this->data[3][1] * _m2(1, 5) + + this->data[3][2] * _m2(2, 5) + + this->data[3][3] * _m2(3, 5) + + this->data[3][4] * _m2(4, 5) + + this->data[3][5] * _m2(5, 5), + + this->data[4][0] * _m2(0, 0) + + this->data[4][1] * _m2(1, 0) + + this->data[4][2] * _m2(2, 0) + + this->data[4][3] * _m2(3, 0) + + this->data[4][4] * _m2(4, 0) + + this->data[4][5] * _m2(5, 0), + + this->data[4][0] * _m2(0, 1) + + this->data[4][1] * _m2(1, 1) + + this->data[4][2] * _m2(2, 1) + + this->data[4][3] * _m2(3, 1) + + this->data[4][4] * _m2(4, 1) + + this->data[4][5] * _m2(5, 1), + + this->data[4][0] * _m2(0, 2) + + this->data[4][1] * _m2(1, 2) + + this->data[4][2] * _m2(2, 2) + + this->data[4][3] * _m2(3, 2) + + this->data[4][4] * _m2(4, 2) + + this->data[4][5] * _m2(5, 2), + + this->data[4][0] * _m2(0, 3) + + this->data[4][1] * _m2(1, 3) + + this->data[4][2] * _m2(2, 3) + + this->data[4][3] * _m2(3, 3) + + this->data[4][4] * _m2(4, 3) + + this->data[4][5] * _m2(5, 3), + + this->data[4][0] * _m2(0, 4) + + this->data[4][1] * _m2(1, 4) + + this->data[4][2] * _m2(2, 4) + + this->data[4][3] * _m2(3, 4) + + this->data[4][4] * _m2(4, 4) + + this->data[4][5] * _m2(5, 4), + + this->data[4][0] * _m2(0, 5) + + this->data[4][1] * _m2(1, 5) + + this->data[4][2] * _m2(2, 5) + + this->data[4][3] * _m2(3, 5) + + this->data[4][4] * _m2(4, 5) + + this->data[4][5] * _m2(5, 5), + + this->data[5][0] * _m2(0, 0) + + this->data[5][1] * _m2(1, 0) + + this->data[5][2] * _m2(2, 0) + + this->data[5][3] * _m2(3, 0) + + this->data[5][4] * _m2(4, 0) + + this->data[5][5] * _m2(5, 0), + + this->data[5][0] * _m2(0, 1) + + this->data[5][1] * _m2(1, 1) + + this->data[5][2] * _m2(2, 1) + + this->data[5][3] * _m2(3, 1) + + this->data[5][4] * _m2(4, 1) + + this->data[5][5] * _m2(5, 1), + + this->data[5][0] * _m2(0, 2) + + this->data[5][1] * _m2(1, 2) + + this->data[5][2] * _m2(2, 2) + + this->data[5][3] * _m2(3, 2) + + this->data[5][4] * _m2(4, 2) + + this->data[5][5] * _m2(5, 2), + + this->data[5][0] * _m2(0, 3) + + this->data[5][1] * _m2(1, 3) + + this->data[5][2] * _m2(2, 3) + + this->data[5][3] * _m2(3, 3) + + this->data[5][4] * _m2(4, 3) + + this->data[5][5] * _m2(5, 3), + + this->data[5][0] * _m2(0, 4) + + this->data[5][1] * _m2(1, 4) + + this->data[5][2] * _m2(2, 4) + + this->data[5][3] * _m2(3, 4) + + this->data[5][4] * _m2(4, 4) + + this->data[5][5] * _m2(5, 4), + + this->data[5][0] * _m2(0, 5) + + this->data[5][1] * _m2(1, 5) + + this->data[5][2] * _m2(2, 5) + + this->data[5][3] * _m2(3, 5) + + this->data[5][4] * _m2(4, 5) + + this->data[5][5] * _m2(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, 3]. + /// \param[in] _row the row index. Index values are clamped to a + /// range of [0, 3]. + /// \return The value at the specified index + public: inline const T &operator()(const size_t _row, + const size_t _col) const + { + return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)][ + clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; + } + + /// \brief Get a mutable version the value at the specified row, + /// column index + /// \param[in] _col The column index. Index values are clamped to a + /// range of [0, 3]. + /// \param[in] _row the row index. Index values are clamped to a + /// range of [0, 3]. + /// \return The value at the specified index + public: inline T &operator()(const size_t _row, const size_t _col) + { + return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; + } + + /// \brief Equality test with tolerance. + /// \param[in] _m the matrix to compare to + /// \param[in] _tol equality tolerance. + /// \return true if the elements of the matrices are equal within + /// the tolerence specified by _tol. + public: bool Equal(const Matrix6 &_m, const T &_tol) const + { + return equal(this->data[0][0], _m(0, 0), _tol) + && equal(this->data[0][1], _m(0, 1), _tol) + && equal(this->data[0][2], _m(0, 2), _tol) + && equal(this->data[0][3], _m(0, 3), _tol) + && equal(this->data[0][4], _m(0, 4), _tol) + && equal(this->data[0][5], _m(0, 5), _tol) + && equal(this->data[1][0], _m(1, 0), _tol) + && equal(this->data[1][1], _m(1, 1), _tol) + && equal(this->data[1][2], _m(1, 2), _tol) + && equal(this->data[1][3], _m(1, 3), _tol) + && equal(this->data[1][4], _m(1, 4), _tol) + && equal(this->data[1][5], _m(1, 5), _tol) + && equal(this->data[2][0], _m(2, 0), _tol) + && equal(this->data[2][1], _m(2, 1), _tol) + && equal(this->data[2][2], _m(2, 2), _tol) + && equal(this->data[2][3], _m(2, 3), _tol) + && equal(this->data[2][4], _m(2, 4), _tol) + && equal(this->data[2][5], _m(2, 5), _tol) + && equal(this->data[3][0], _m(3, 0), _tol) + && equal(this->data[3][1], _m(3, 1), _tol) + && equal(this->data[3][2], _m(3, 2), _tol) + && equal(this->data[3][3], _m(3, 3), _tol) + && equal(this->data[3][4], _m(3, 4), _tol) + && equal(this->data[3][5], _m(3, 5), _tol) + && equal(this->data[4][0], _m(4, 0), _tol) + && equal(this->data[4][1], _m(4, 1), _tol) + && equal(this->data[4][2], _m(4, 2), _tol) + && equal(this->data[4][3], _m(4, 3), _tol) + && equal(this->data[4][4], _m(4, 4), _tol) + && equal(this->data[4][5], _m(4, 5), _tol) + && equal(this->data[5][0], _m(5, 0), _tol) + && equal(this->data[5][1], _m(5, 1), _tol) + && equal(this->data[5][2], _m(5, 2), _tol) + && equal(this->data[5][3], _m(5, 3), _tol) + && equal(this->data[5][4], _m(5, 4), _tol) + && equal(this->data[5][5], _m(5, 5), _tol); + } + + /// \brief Equality operator + /// \param[in] _m Matrix3 to test + /// \return true if the 2 matrices are equal (using the tolerance 1e-6), + /// false otherwise + public: bool operator==(const Matrix6 &_m) const + { + return this->Equal(_m, static_cast(1e-6)); + } + + /// \brief Inequality test operator + /// \param[in] _m Matrix6 to test + /// \return True if not equal (using the default tolerance of 1e-6) + public: bool operator!=(const Matrix6 &_m) const + { + return !(*this == _m); + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _m Matrix to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const ignition::math::Matrix6 &_m) + { + for (auto i : {0, 1, 2, 3, 4, 5}) + { + for (auto j : {0, 1, 2, 3, 4, 5}) + { + if (!(i == 0 && j == 0)) + _out << " "; + + appendToStream(_out, _m(i, j)); + } + } + + return _out; + } + + /// \brief Stream extraction operator + /// \param[in,out] _in input stream + /// \param[out] _m Matrix6 to read values into + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, ignition::math::Matrix6 &_m) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + T d[36]; + _in >> d[0] >> d[1] >> d[2] >> d[3] >> d[4] >> d[5] + >> d[6] >> d[7] >> d[8] >> d[9] >> d[10] >> d[11] + >> d[12] >> d[13] >> d[14] >> d[15] >> d[16] >> d[17] + >> d[18] >> d[19] >> d[20] >> d[21] >> d[22] >> d[23] + >> d[24] >> d[25] >> d[26] >> d[27] >> d[28] >> d[29] + >> d[30] >> d[31] >> d[32] >> d[33] >> d[34] >> d[35]; + + if (!_in.fail()) + { + _m.Set(d[0], d[1], d[2], d[3], d[4], d[5], + d[6], d[7], d[8], d[9], d[10], d[11], + d[12], d[13], d[14], d[15], d[16], d[17], + d[18], d[19], d[20], d[21], d[22], d[23], + d[24], d[25], d[26], d[27], d[28], d[29], + d[30], d[31], d[32], d[33], d[34], d[35]); + } + return _in; + } + + /// \brief The 6x6 matrix + private: T data[MatrixSize][MatrixSize]; + }; + + namespace detail { + + template + constexpr Matrix6 gMatrix6Identity( + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1); + + template + constexpr Matrix6 gMatrix6Zero( + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0); + + } // namespace detail + + template + const Matrix6 &Matrix6::Identity = detail::gMatrix6Identity; + + template + const Matrix6 &Matrix6::Zero = detail::gMatrix6Zero; + + typedef Matrix6 Matrix6i; + typedef Matrix6 Matrix6d; + typedef Matrix6 Matrix6f; + } + } +} +#endif diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc new file mode 100644 index 000000000..ce84a74b7 --- /dev/null +++ b/src/Matrix6_TEST.cc @@ -0,0 +1,277 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "ignition/math/Matrix6.hh" + +using namespace ignition; +using namespace math; + +///////////////////////////////////////////////// +TEST(Matrix6dTest, Construct) +{ + Matrix6d mat; + for (int i = 0; i < 6; ++i) + { + for (int j = 0; j < 6; ++j) + { + EXPECT_DOUBLE_EQ(mat(i, j), 0.0); + } + } + + Matrix6d mat2(mat); + for (int i = 0; i < 6; ++i) + { + for (int j = 0; j < 6; ++j) + { + EXPECT_DOUBLE_EQ(mat2(i, j), 0.0); + } + } + EXPECT_TRUE(mat2 == mat); + + // Set individual values. + Matrix6d mat3( + 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); + + Matrix6d mat4; + mat4 = mat3; + EXPECT_EQ(mat4, mat3); + + EXPECT_DOUBLE_EQ(mat3(0, 0), 0.0); + EXPECT_DOUBLE_EQ(mat3(0, 1), 1.0); + EXPECT_DOUBLE_EQ(mat3(0, 2), 2.0); + EXPECT_DOUBLE_EQ(mat3(0, 3), 3.0); + EXPECT_DOUBLE_EQ(mat3(0, 4), 4.0); + EXPECT_DOUBLE_EQ(mat3(0, 5), 5.0); + EXPECT_DOUBLE_EQ(mat3(1, 0), 6.0); + EXPECT_DOUBLE_EQ(mat3(1, 1), 7.0); + EXPECT_DOUBLE_EQ(mat3(1, 2), 8.0); + EXPECT_DOUBLE_EQ(mat3(1, 3), 9.0); + EXPECT_DOUBLE_EQ(mat3(1, 4), 10.0); + EXPECT_DOUBLE_EQ(mat3(1, 5), 11.0); + EXPECT_DOUBLE_EQ(mat3(2, 0), 12.0); + EXPECT_DOUBLE_EQ(mat3(2, 1), 13.0); + EXPECT_DOUBLE_EQ(mat3(2, 2), 14.0); + EXPECT_DOUBLE_EQ(mat3(2, 3), 15.0); + EXPECT_DOUBLE_EQ(mat3(2, 4), 16.0); + EXPECT_DOUBLE_EQ(mat3(2, 5), 17.0); + EXPECT_DOUBLE_EQ(mat3(3, 0), 18.0); + EXPECT_DOUBLE_EQ(mat3(3, 1), 19.0); + EXPECT_DOUBLE_EQ(mat3(3, 2), 20.0); + EXPECT_DOUBLE_EQ(mat3(3, 3), 21.0); + EXPECT_DOUBLE_EQ(mat3(3, 4), 22.0); + EXPECT_DOUBLE_EQ(mat3(3, 5), 23.0); + EXPECT_DOUBLE_EQ(mat3(4, 0), 24.0); + EXPECT_DOUBLE_EQ(mat3(4, 1), 25.0); + EXPECT_DOUBLE_EQ(mat3(4, 2), 26.0); + EXPECT_DOUBLE_EQ(mat3(4, 3), 27.0); + EXPECT_DOUBLE_EQ(mat3(4, 4), 28.0); + EXPECT_DOUBLE_EQ(mat3(4, 5), 29.0); + EXPECT_DOUBLE_EQ(mat3(5, 0), 30.0); + EXPECT_DOUBLE_EQ(mat3(5, 1), 31.0); + EXPECT_DOUBLE_EQ(mat3(5, 2), 32.0); + EXPECT_DOUBLE_EQ(mat3(5, 3), 33.0); + EXPECT_DOUBLE_EQ(mat3(5, 4), 34.0); + EXPECT_DOUBLE_EQ(mat3(5, 5), 35.0); + EXPECT_DOUBLE_EQ(mat3(100, 100), 35.0); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, CoverageExtra) +{ + // getting full destructor coverage + auto *p = new Matrix6d; + EXPECT_NE(p, nullptr); + delete p; +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, Multiply4) +{ + 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; + } + } + + // Answer checked with + // https://www.emathhelp.net/calculators/linear-algebra/matrix-multiplication-calculator/?a=%5B%5B0%2C-1%2C-2%2C-3%2C-4%2C-5%5D%2C%5B1%2C0%2C-1%2C-2%2C-3%2C-4%5D%2C%5B2%2C1%2C0%2C-1%2C-2%2C-3%5D%2C%5B3%2C2%2C1%2C0%2C-1%2C-2%5D%2C%5B4%2C3%2C2%2C1%2C0%2C-1%5D%2C%5B5%2C4%2C3%2C2%2C1%2C0%5D%5D&b=%5B%5B0%2C1%2C2%2C3%2C4%2C5%5D%2C%5B1%2C2%2C3%2C4%2C5%2C6%5D%2C%5B2%2C3%2C4%2C5%2C6%2C7%5D%2C%5B3%2C4%2C5%2C6%2C7%2C8%5D%2C%5B4%2C5%2C6%2C7%2C8%2C9%5D%2C%5B5%2C6%2C7%2C8%2C9%2C10%5D%5D + Matrix6d mat3( + -55, -70, -85, -100, -115, -130, + -40, -49, -58, -67, -76, -85, + -25, -28, -31, -34, -37, -40, + -10, -7, -4, -1, 2, 5, + 5, 14, 23, 32, 41, 50, + 20, 35, 50, 65, 80, 95); + + auto mat2 = mat * mat1; + EXPECT_EQ(mat2, mat3); + + auto mat4 = mat; + mat4 *= mat1; + EXPECT_EQ(mat2, mat4); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, NoIndexException) +{ + auto mat = Matrix6d::Zero; + for (int i = 0; i < 6; ++i) + for (int j = 0; j < 6; ++j) + EXPECT_NO_THROW(mat(i, j)); + + EXPECT_NO_THROW(equal(mat(6, 0), 0.0)); + EXPECT_NO_THROW(equal(mat(0, 6), 0.0)); + EXPECT_NO_THROW(equal(mat(6, 6), 0.0)); + + EXPECT_NO_THROW(mat(6, 0) = 0); + EXPECT_NO_THROW(mat(0, 6) = 0); + EXPECT_NO_THROW(mat(6, 6) = 0); + + const Matrix6d constMat(Matrix6d::Zero); + + EXPECT_NO_THROW(equal(constMat(6, 0), 0.0)); + EXPECT_NO_THROW(equal(constMat(0, 6), 0.0)); + EXPECT_NO_THROW(equal(constMat(6, 6), 0.0)); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, OperatorStreamOut) +{ + Matrix6d matA( + 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); + + std::ostringstream stream; + stream << matA; + EXPECT_EQ(stream.str(), + "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"); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, OperatorStreamIn) +{ + Matrix6d mat; + EXPECT_EQ(mat, Matrix6d::Zero); + + std::istringstream stream( + "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"); + stream >> mat; + EXPECT_EQ(mat, 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)); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, NotEqual) +{ + { + Matrix6d matrix1; + Matrix6d matrix2; + EXPECT_TRUE(matrix1 == matrix2); + EXPECT_FALSE(matrix1 != matrix2); + } + + { + Matrix6d matrix1( + 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); + Matrix6d matrix2(matrix1); + + EXPECT_FALSE(matrix1 != matrix1); + + matrix2(0, 0) = 0.00001; + EXPECT_TRUE(matrix1 != matrix2); + + matrix2(0, 0) = 0.000001; + EXPECT_FALSE(matrix1 != matrix2); + } +} + +///////////////////////////////////////////////// +// Test Equal function with specified tolerance +TEST(Matrix6Test, EqualTolerance) +{ + EXPECT_FALSE(Matrix6d::Zero.Equal(Matrix6d::Identity, 1e-6)); + EXPECT_FALSE(Matrix6d::Zero.Equal(Matrix6d::Identity, 1e-3)); + EXPECT_FALSE(Matrix6d::Zero.Equal(Matrix6d::Identity, 1e-1)); + EXPECT_TRUE(Matrix6d::Zero.Equal(Matrix6d::Identity, 1)); + EXPECT_TRUE(Matrix6d::Zero.Equal(Matrix6d::Identity, 1.1)); +} + +///////////////////////////////////////////////// +TEST(Matrix6dTest, Transpose) +{ + // Transpose of zero matrix is itself + EXPECT_EQ(Matrix6d::Zero, Matrix6d::Zero.Transposed()); + + // Transpose of identity matrix is itself + EXPECT_EQ(Matrix6d::Identity, Matrix6d::Identity.Transposed()); + + // Matrix and expected transpose + Matrix6d m(-2, 4, 0, -3.5, 4.1, 2.9, + 0.1, 9, 55, 1.2, 10.0, -0.02, + -7, 1, 26, 11.5, 6.9, -5.9, + .2, 3, -5, -0.1, 3.3, 0.0, + 0.2, -9.8, 100, 5.6, 7.8, 0.002, + 9.8, 5.4, -9.7, -0.1, 6.22, 5.41); + Matrix6d mT(-2, 0.1, -7, .2, 0.2, 9.8, + 4, 9, 1, 3, -9.8, 5.4, + 0, 55, 26, -5, 100, -9.7, + -3.5, 1.2, 11.5, -0.1, 5.6, -0.1, + 4.1, 10.0, 6.9, 3.3, 7.8, 6.22, + 2.9, -0.02, -5.9, 0.0, 0.002, 5.41); + EXPECT_NE(m, mT); + EXPECT_EQ(m.Transposed(), mT); + + mT.Transpose(); + EXPECT_EQ(m, mT); +} + From 2be91fb6e0a4260cfe4e385558cd16e5656f2306 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Jul 2022 20:28:39 -0700 Subject: [PATCH 03/10] Submatrix Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 126 +++++++++++++++++++++++++------ src/Matrix6_TEST.cc | 32 ++++++++ 2 files changed, 134 insertions(+), 24 deletions(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 2027c1584..58d2564de 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -19,6 +19,7 @@ #include #include +#include namespace ignition { @@ -32,6 +33,26 @@ namespace ignition template class Matrix6 { + /// \brief Identifiers for each of the 4 3x3 corners of the matrix. + public: enum Corner + { + /// \brief Top-left corner, consisting of the intersection between the + /// first 3 rows and first 3 columns. + TOP_LEFT = 1, + + /// \brief Top-right corner, consisting of the intersection between the + /// first 3 rows and last 3 columns. + TOP_RIGHT = 2, + + /// \brief Bottom-left corner, consisting of the intersection between the + /// last 3 rows and first 3 columns. + BOTTOM_LEFT = 3, + + /// \brief Bottom-right corner, consisting of the intersection between the + /// last 3 rows and last 3 columns. + BOTTOM_RIGHT = 4 + }; + /// \brief Size of matrix is fixed to 6x6 public: static constexpr std::size_t MatrixSize{6}; @@ -196,18 +217,47 @@ namespace ignition public: Matrix6 Transposed() const { return Matrix6( - this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0], - this->data[4][0], this->data[5][0], - this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1], - this->data[4][1], this->data[5][1], - this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2], - this->data[4][2], this->data[5][2], - this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3], - this->data[4][3], this->data[5][3], - this->data[0][4], this->data[1][4], this->data[2][4], this->data[3][4], - this->data[4][4], this->data[5][4], - this->data[0][5], this->data[1][5], this->data[2][5], this->data[3][5], - this->data[4][5], this->data[5][5]); + this->data[0][0], + this->data[1][0], + this->data[2][0], + this->data[3][0], + this->data[4][0], + this->data[5][0], + + this->data[0][1], + this->data[1][1], + this->data[2][1], + this->data[3][1], + this->data[4][1], + this->data[5][1], + + this->data[0][2], + this->data[1][2], + this->data[2][2], + this->data[3][2], + this->data[4][2], + this->data[5][2], + + this->data[0][3], + this->data[1][3], + this->data[2][3], + this->data[3][3], + this->data[4][3], + this->data[5][3], + + this->data[0][4], + this->data[1][4], + this->data[2][4], + this->data[3][4], + this->data[4][4], + this->data[5][4], + + this->data[0][5], + this->data[1][5], + this->data[2][5], + this->data[3][5], + this->data[4][5], + this->data[5][5]); } /// \brief Assignment operator. this = _mat @@ -497,18 +547,46 @@ namespace ignition clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; } - /// \brief Get a mutable version the value at the specified row, - /// column index - /// \param[in] _col The column index. Index values are clamped to a - /// range of [0, 3]. - /// \param[in] _row the row index. Index values are clamped to a - /// range of [0, 3]. - /// \return The value at the specified index - public: inline T &operator()(const size_t _row, const size_t _col) - { - return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)] - [clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; - } + /// \brief Get a mutable version the value at the specified row, + /// column index + /// \param[in] _row the row index. Index values are clamped to a + /// range of [0, 3]. + /// \param[in] _col The column index. Index values are clamped to a + /// range of [0, 3]. + /// \return The value at the specified index + public: inline T &operator()(const size_t _row, const size_t _col) + { + return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; + } + + /// \brief Get one of the four 3x3 submatrices that compose this matrix. + /// These submatrices are formed by dividing the 6x6 matrix in 4 parts and + /// do not overlap with each other. + /// \param[in] _corner Which corner to retrieve. + /// \return A new matrix containing the values of the submatrix. + public: Matrix3 Submatrix(Corner _corner) const + { + size_t row = 0; + size_t col = 0; + if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT) + { + row = 3; + } + if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT) + { + col = 3; + } + return {this->data[row + 0][col + 0], + this->data[row + 0][col + 1], + this->data[row + 0][col + 2], + this->data[row + 1][col + 0], + this->data[row + 1][col + 1], + this->data[row + 1][col + 2], + this->data[row + 2][col + 0], + this->data[row + 2][col + 1], + this->data[row + 2][col + 2]}; + } /// \brief Equality test with tolerance. /// \param[in] _m the matrix to compare to diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index ce84a74b7..359f8493d 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -275,3 +275,35 @@ TEST(Matrix6dTest, Transpose) EXPECT_EQ(m, mT); } +///////////////////////////////////////////////// +TEST(Matrix6dTest, Submatrix) +{ + Matrix6i mat( + 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); + + EXPECT_EQ(mat.Submatrix(Matrix6i::TOP_LEFT), Matrix3i( + 0, 1, 2, + 6, 7, 8, + 12, 13, 14)); + + EXPECT_EQ(mat.Submatrix(Matrix6i::TOP_RIGHT), Matrix3i( + 3, 4, 5, + 9, 10, 11, + 15, 16, 17)); + + EXPECT_EQ(mat.Submatrix(Matrix6i::BOTTOM_LEFT), Matrix3i( + 18, 19, 20, + 24, 25, 26, + 30, 31, 32)); + + EXPECT_EQ(mat.Submatrix(Matrix6i::BOTTOM_RIGHT), Matrix3i( + 21, 22, 23, + 27, 28, 29, + 33, 34, 35)); +} + From f29aed18c2361a04d44324e915c1e3e9adb8ca68 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Jul 2022 20:31:41 -0700 Subject: [PATCH 04/10] codecheck Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 58d2564de..d40e822bd 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -17,6 +17,7 @@ #ifndef GZ_MATH_MATRIX6_HH_ #define GZ_MATH_MATRIX6_HH_ +#include #include #include #include @@ -44,12 +45,12 @@ namespace ignition /// first 3 rows and last 3 columns. TOP_RIGHT = 2, - /// \brief Bottom-left corner, consisting of the intersection between the - /// last 3 rows and first 3 columns. + /// \brief Bottom-left corner, consisting of the intersection between + /// the last 3 rows and first 3 columns. BOTTOM_LEFT = 3, - /// \brief Bottom-right corner, consisting of the intersection between the - /// last 3 rows and last 3 columns. + /// \brief Bottom-right corner, consisting of the intersection between + /// the last 3 rows and last 3 columns. BOTTOM_RIGHT = 4 }; From b8987a6a4641f744c3b23e78332c5e880cecc929 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Jul 2022 22:01:59 -0700 Subject: [PATCH 05/10] Python Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 12 +- src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Matrix6.cc | 57 +++++ src/python_pybind11/src/Matrix6.hh | 100 +++++++++ .../src/_ignition_math_pybind11.cc | 3 + src/python_pybind11/test/Matrix6_TEST.py | 203 ++++++++++++++++++ 6 files changed, 370 insertions(+), 6 deletions(-) create mode 100644 src/python_pybind11/src/Matrix6.cc create mode 100644 src/python_pybind11/src/Matrix6.hh create mode 100644 src/python_pybind11/test/Matrix6_TEST.py diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index d40e822bd..74f2788ac 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -35,23 +35,23 @@ namespace ignition class Matrix6 { /// \brief Identifiers for each of the 4 3x3 corners of the matrix. - public: enum Corner + public: enum Matrix6Corner { /// \brief Top-left corner, consisting of the intersection between the /// first 3 rows and first 3 columns. - TOP_LEFT = 1, + TOP_LEFT = 0, /// \brief Top-right corner, consisting of the intersection between the /// first 3 rows and last 3 columns. - TOP_RIGHT = 2, + TOP_RIGHT = 1, /// \brief Bottom-left corner, consisting of the intersection between /// the last 3 rows and first 3 columns. - BOTTOM_LEFT = 3, + BOTTOM_LEFT = 2, /// \brief Bottom-right corner, consisting of the intersection between /// the last 3 rows and last 3 columns. - BOTTOM_RIGHT = 4 + BOTTOM_RIGHT = 3 }; /// \brief Size of matrix is fixed to 6x6 @@ -566,7 +566,7 @@ namespace ignition /// do not overlap with each other. /// \param[in] _corner Which corner to retrieve. /// \return A new matrix containing the values of the submatrix. - public: Matrix3 Submatrix(Corner _corner) const + public: Matrix3 Submatrix(Matrix6Corner _corner) const { size_t row = 0; size_t col = 0; diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 5967f17dc..7c7c10d63 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -26,6 +26,7 @@ pybind11_add_module(math SHARED src/Material.cc src/Matrix3.cc src/Matrix4.cc + src/Matrix6.cc src/MovingWindowFilter.cc src/PID.cc src/Pose3.cc diff --git a/src/python_pybind11/src/Matrix6.cc b/src/python_pybind11/src/Matrix6.cc new file mode 100644 index 000000000..47af0a4ae --- /dev/null +++ b/src/python_pybind11/src/Matrix6.cc @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "Matrix6.hh" + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathMatrix6(py::module &_m, const std::string &_typestr) +{ + helpDefineMathMatrix6(_m, _typestr + "d"); + py::enum_(_m, (_typestr + "dCorner").c_str()) + .value("TOP_LEFT", Matrix6d::Matrix6Corner::TOP_LEFT) + .value("TOP_RIGHT", Matrix6d::Matrix6Corner::TOP_RIGHT) + .value("BOTTOM_LEFT", Matrix6d::Matrix6Corner::BOTTOM_LEFT) + .value("BOTTOM_RIGHT", Matrix6d::Matrix6Corner::BOTTOM_RIGHT) + .export_values(); + + helpDefineMathMatrix6(_m, _typestr + "f"); + py::enum_(_m, (_typestr + "fCorner").c_str()) + .value("TOP_LEFT", Matrix6f::Matrix6Corner::TOP_LEFT) + .value("TOP_RIGHT", Matrix6f::Matrix6Corner::TOP_RIGHT) + .value("BOTTOM_LEFT", Matrix6f::Matrix6Corner::BOTTOM_LEFT) + .value("BOTTOM_RIGHT", Matrix6f::Matrix6Corner::BOTTOM_RIGHT) + .export_values(); + + helpDefineMathMatrix6(_m, _typestr + "i"); + py::enum_(_m, (_typestr + "iCorner").c_str()) + .value("TOP_LEFT", Matrix6i::Matrix6Corner::TOP_LEFT) + .value("TOP_RIGHT", Matrix6i::Matrix6Corner::TOP_RIGHT) + .value("BOTTOM_LEFT", Matrix6i::Matrix6Corner::BOTTOM_LEFT) + .value("BOTTOM_RIGHT", Matrix6i::Matrix6Corner::BOTTOM_RIGHT) + .export_values(); +} + +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Matrix6.hh b/src/python_pybind11/src/Matrix6.hh new file mode 100644 index 000000000..1785adf7f --- /dev/null +++ b/src/python_pybind11/src/Matrix6.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * 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 + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_MATH_PYTHON__MATRIX6_HH_ +#define GZ_MATH_PYTHON__MATRIX6_HH_ + +#include + +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an Matrix6 +/** + * \param[in] _m a pybind11 module to add the definition to + * \param[in] _typestr name of the type used by Python + */ +void defineMathMatrix6(py::module &_m, const std::string &_typestr); + +/// Define a pybind11 wrapper for an Matrix6 +/** + * \param[in] _m a pybind11 module to add the definition to + * \param[in] _typestr name of the type used by Python + */ +template +void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr) +{ + using Class = Matrix6; + auto toString = [](const Class &_si) { + std::stringstream stream; + stream << _si; + return stream.str(); + }; + py::class_(_m, + _typestr.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::self * py::self) + .def(py::self == py::self) + .def(py::self != py::self) + .def("__call__", + py::overload_cast(&Class::operator()), + py::return_value_policy::reference_internal) + .def("set", + &Class::Set, + "Set values") + .def("equal", + &Class::Equal, + "Equality test operator") + .def("submatrix", + &Class::Submatrix, + "One of the four 3x3 submatrices that compose this matrix.") + .def("__copy__", [](const Class &_self) { + return Class(_self); + }) + .def("__deepcopy__", [](const Class &_self, py::dict) { + return Class(_self); + }, "memo"_a) + .def_readonly_static("IDENTITY", &Class::Identity, "Identity matrix") + .def_readonly_static("ZERO", &Class::Zero, "Zero matrix") + .def("__str__", toString) + .def("__repr__", toString); +} +} // namespace python +} // namespace math +} // namespace ignition + +#endif // GZ_MATH_PYTHON__MATRIX6_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 00497c61c..3d4195ff4 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -34,6 +34,7 @@ #include "Material.hh" #include "Matrix3.hh" #include "Matrix4.hh" +#include "Matrix6.hh" #include "MovingWindowFilter.hh" #include "OrientedBox.hh" #include "PID.hh" @@ -139,6 +140,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathMatrix4(m, "Matrix4"); + ignition::math::python::defineMathMatrix6(m, "Matrix6"); + ignition::math::python::defineMathTriangle(m, "Triangle"); ignition::math::python::defineMathTriangle3(m, "Triangle3"); diff --git a/src/python_pybind11/test/Matrix6_TEST.py b/src/python_pybind11/test/Matrix6_TEST.py new file mode 100644 index 000000000..f307253ae --- /dev/null +++ b/src/python_pybind11/test/Matrix6_TEST.py @@ -0,0 +1,203 @@ +# Copyright (C) 2022 Open Source Robotics Foundation +# +# 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 +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Matrix3d, Matrix6d, Matrix6dCorner + + +class TestMatrix6(unittest.TestCase): + def test_construct(self): + mat = Matrix6d() + + for i in range(7): + for j in range(7): + self.assertAlmostEqual(mat(i, j), 0.0) + + mat2 = Matrix6d(mat) + for i in range(7): + for j in range(7): + self.assertAlmostEqual(mat2(i, j), 0.0) + + self.assertTrue(mat2 == mat) + + mat3 = 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); + + 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); + + def test_multiply_mat(self): + mat = Matrix6d(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) + mat1 = Matrix6d(0, 1, 2, 3, 4, 5, + 1, 2, 3, 4, 5, 6, + 2, 3, 4, 5, 6, 7, + 3, 4, 5, 6, 7, 8, + 4, 5, 6, 7, 8, 9, + 5, 6, 7, 8, 9, 10) + + mat3 = Matrix6d( + -55, -70, -85, -100, -115, -130, + -40, -49, -58, -67, -76, -85, + -25, -28, -31, -34, -37, -40, + -10, -7, -4, -1, 2, 5, + 5, 14, 23, 32, 41, 50, + 20, 35, 50, 65, 80, 95) + + 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); + + self.assertEqual(str(matA), + "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") + + def test_not_equal(self): + matrix1 = Matrix6d() + matrix2 = Matrix6d() + self.assertTrue(matrix1 == matrix2) + self.assertFalse(matrix1 != matrix2) + + matrix1 = 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) + matrix2 = Matrix6d(matrix1) + + self.assertFalse(matrix1 != matrix2) + + matrix2 = Matrix6d( + 0.00001, 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) + self.assertTrue(matrix1 != matrix2) + + matrix2 = Matrix6d( + 0.000001, 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) + self.assertFalse(matrix1 != matrix2) + + def test_equal_tolerance(self): + self.assertFalse(Matrix6d.ZERO.equal(Matrix6d.IDENTITY, 1e-6)) + self.assertFalse(Matrix6d.ZERO.equal(Matrix6d.IDENTITY, 1e-3)) + self.assertFalse(Matrix6d.ZERO.equal(Matrix6d.IDENTITY, 1e-1)) + self.assertTrue(Matrix6d.ZERO.equal(Matrix6d.IDENTITY, 1)) + self.assertTrue(Matrix6d.ZERO.equal(Matrix6d.IDENTITY, 1.1)) + + def test_submatrix(self): + 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); + + self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_LEFT), Matrix3d( + 0, 1, 2, + 6, 7, 8, + 12, 13, 14)); + + self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_RIGHT), Matrix3d( + 3, 4, 5, + 9, 10, 11, + 15, 16, 17)); + + self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_LEFT), Matrix3d( + 18, 19, 20, + 24, 25, 26, + 30, 31, 32)); + + self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_RIGHT), Matrix3d( + 21, 22, 23, + 27, 28, 29, + 33, 34, 35)); + +if __name__ == '__main__': + unittest.main() From 2e3946c2aa2721081f8b05f29af7a0511f6a2e7d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 6 Jul 2022 08:57:26 -0700 Subject: [PATCH 06/10] doxy Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 74f2788ac..32877f476 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -130,18 +130,38 @@ namespace ignition /// \param[in] _v01 Row 0, Col 1 value /// \param[in] _v02 Row 0, Col 2 value /// \param[in] _v03 Row 0, Col 3 value + /// \param[in] _v04 Row 0, Col 4 value + /// \param[in] _v05 Row 0, Col 5 value /// \param[in] _v10 Row 1, Col 0 value /// \param[in] _v11 Row 1, Col 1 value /// \param[in] _v12 Row 1, Col 2 value /// \param[in] _v13 Row 1, Col 3 value + /// \param[in] _v14 Row 1, Col 4 value + /// \param[in] _v15 Row 1, Col 5 value /// \param[in] _v20 Row 2, Col 0 value /// \param[in] _v21 Row 2, Col 1 value /// \param[in] _v22 Row 2, Col 2 value /// \param[in] _v23 Row 2, Col 3 value + /// \param[in] _v24 Row 2, Col 4 value + /// \param[in] _v25 Row 2, Col 5 value /// \param[in] _v30 Row 3, Col 0 value /// \param[in] _v31 Row 3, Col 1 value /// \param[in] _v32 Row 3, Col 2 value /// \param[in] _v33 Row 3, Col 3 value + /// \param[in] _v34 Row 3, Col 4 value + /// \param[in] _v35 Row 3, Col 5 value + /// \param[in] _v40 Row 4, Col 0 value + /// \param[in] _v41 Row 4, Col 1 value + /// \param[in] _v42 Row 4, Col 2 value + /// \param[in] _v43 Row 4, Col 3 value + /// \param[in] _v44 Row 4, Col 4 value + /// \param[in] _v45 Row 4, Col 5 value + /// \param[in] _v50 Row 5, Col 0 value + /// \param[in] _v51 Row 5, Col 1 value + /// \param[in] _v52 Row 5, Col 2 value + /// \param[in] _v53 Row 5, Col 3 value + /// \param[in] _v54 Row 5, Col 4 value + /// \param[in] _v55 Row 5, Col 5 value public: void Set( T _v00, T _v01, T _v02, T _v03, T _v04, T _v05, T _v10, T _v11, T _v12, T _v13, T _v14, T _v15, From efb0f0d9726ec7128f9077d2e1698f86c0195585 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 Jul 2022 10:05:55 -0700 Subject: [PATCH 07/10] Reduce duplication in multiplication, add SetSubmatrix Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 308 ++++------------------- src/Matrix6_TEST.cc | 34 +++ src/python_pybind11/src/Matrix6.hh | 3 + src/python_pybind11/test/Matrix6_TEST.py | 31 +++ 4 files changed, 116 insertions(+), 260 deletions(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 32877f476..67a0320ec 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -289,7 +289,7 @@ namespace ignition /// \brief Multiplication assignment operator. This matrix will /// become equal to this * _m2. /// \param[in] _m2 Incoming matrix. - /// \return This matrix * _mat. + /// \return This matrix * _m2. public: Matrix6 operator*=(const Matrix6 &_m2) { (*this) = (*this) * _m2; @@ -298,268 +298,30 @@ namespace ignition /// \brief Multiplication operator /// \param[in] _m2 Incoming matrix - /// \return This matrix * _mat + /// \return This matrix * _m2 public: Matrix6 operator*(const Matrix6 &_m2) const { + auto el = [&](size_t _row, size_t _col) -> T + { + T result = static_cast(0); + for (size_t i = 0; i < MatrixSize; ++i) + result += this->data[_row][i] * _m2(i, _col); + return result; + }; return Matrix6( - this->data[0][0] * _m2(0, 0) + - this->data[0][1] * _m2(1, 0) + - this->data[0][2] * _m2(2, 0) + - this->data[0][3] * _m2(3, 0) + - this->data[0][4] * _m2(4, 0) + - this->data[0][5] * _m2(5, 0), - - this->data[0][0] * _m2(0, 1) + - this->data[0][1] * _m2(1, 1) + - this->data[0][2] * _m2(2, 1) + - this->data[0][3] * _m2(3, 1) + - this->data[0][4] * _m2(4, 1) + - this->data[0][5] * _m2(5, 1), - - this->data[0][0] * _m2(0, 2) + - this->data[0][1] * _m2(1, 2) + - this->data[0][2] * _m2(2, 2) + - this->data[0][3] * _m2(3, 2) + - this->data[0][4] * _m2(4, 2) + - this->data[0][5] * _m2(5, 2), - - this->data[0][0] * _m2(0, 3) + - this->data[0][1] * _m2(1, 3) + - this->data[0][2] * _m2(2, 3) + - this->data[0][3] * _m2(3, 3) + - this->data[0][4] * _m2(4, 3) + - this->data[0][5] * _m2(5, 3), - - this->data[0][0] * _m2(0, 4) + - this->data[0][1] * _m2(1, 4) + - this->data[0][2] * _m2(2, 4) + - this->data[0][3] * _m2(3, 4) + - this->data[0][4] * _m2(4, 4) + - this->data[0][5] * _m2(5, 4), - - this->data[0][0] * _m2(0, 5) + - this->data[0][1] * _m2(1, 5) + - this->data[0][2] * _m2(2, 5) + - this->data[0][3] * _m2(3, 5) + - this->data[0][4] * _m2(4, 5) + - this->data[0][5] * _m2(5, 5), - - this->data[1][0] * _m2(0, 0) + - this->data[1][1] * _m2(1, 0) + - this->data[1][2] * _m2(2, 0) + - this->data[1][3] * _m2(3, 0) + - this->data[1][4] * _m2(4, 0) + - this->data[1][5] * _m2(5, 0), - - this->data[1][0] * _m2(0, 1) + - this->data[1][1] * _m2(1, 1) + - this->data[1][2] * _m2(2, 1) + - this->data[1][3] * _m2(3, 1) + - this->data[1][4] * _m2(4, 1) + - this->data[1][5] * _m2(5, 1), - - this->data[1][0] * _m2(0, 2) + - this->data[1][1] * _m2(1, 2) + - this->data[1][2] * _m2(2, 2) + - this->data[1][3] * _m2(3, 2) + - this->data[1][4] * _m2(4, 2) + - this->data[1][5] * _m2(5, 2), - - this->data[1][0] * _m2(0, 3) + - this->data[1][1] * _m2(1, 3) + - this->data[1][2] * _m2(2, 3) + - this->data[1][3] * _m2(3, 3) + - this->data[1][4] * _m2(4, 3) + - this->data[1][5] * _m2(5, 3), - - this->data[1][0] * _m2(0, 4) + - this->data[1][1] * _m2(1, 4) + - this->data[1][2] * _m2(2, 4) + - this->data[1][3] * _m2(3, 4) + - this->data[1][4] * _m2(4, 4) + - this->data[1][5] * _m2(5, 4), - - this->data[1][0] * _m2(0, 5) + - this->data[1][1] * _m2(1, 5) + - this->data[1][2] * _m2(2, 5) + - this->data[1][3] * _m2(3, 5) + - this->data[1][4] * _m2(4, 5) + - this->data[1][5] * _m2(5, 5), - - this->data[2][0] * _m2(0, 0) + - this->data[2][1] * _m2(1, 0) + - this->data[2][2] * _m2(2, 0) + - this->data[2][3] * _m2(3, 0) + - this->data[2][4] * _m2(4, 0) + - this->data[2][5] * _m2(5, 0), - - this->data[2][0] * _m2(0, 1) + - this->data[2][1] * _m2(1, 1) + - this->data[2][2] * _m2(2, 1) + - this->data[2][3] * _m2(3, 1) + - this->data[2][4] * _m2(4, 1) + - this->data[2][5] * _m2(5, 1), - - this->data[2][0] * _m2(0, 2) + - this->data[2][1] * _m2(1, 2) + - this->data[2][2] * _m2(2, 2) + - this->data[2][3] * _m2(3, 2) + - this->data[2][4] * _m2(4, 2) + - this->data[2][5] * _m2(5, 2), - - this->data[2][0] * _m2(0, 3) + - this->data[2][1] * _m2(1, 3) + - this->data[2][2] * _m2(2, 3) + - this->data[2][3] * _m2(3, 3) + - this->data[2][4] * _m2(4, 3) + - this->data[2][5] * _m2(5, 3), - - this->data[2][0] * _m2(0, 4) + - this->data[2][1] * _m2(1, 4) + - this->data[2][2] * _m2(2, 4) + - this->data[2][3] * _m2(3, 4) + - this->data[2][4] * _m2(4, 4) + - this->data[2][5] * _m2(5, 4), - - this->data[2][0] * _m2(0, 5) + - this->data[2][1] * _m2(1, 5) + - this->data[2][2] * _m2(2, 5) + - this->data[2][3] * _m2(3, 5) + - this->data[2][4] * _m2(4, 5) + - this->data[2][5] * _m2(5, 5), - - this->data[3][0] * _m2(0, 0) + - this->data[3][1] * _m2(1, 0) + - this->data[3][2] * _m2(2, 0) + - this->data[3][3] * _m2(3, 0) + - this->data[3][4] * _m2(4, 0) + - this->data[3][5] * _m2(5, 0), - - this->data[3][0] * _m2(0, 1) + - this->data[3][1] * _m2(1, 1) + - this->data[3][2] * _m2(2, 1) + - this->data[3][3] * _m2(3, 1) + - this->data[3][4] * _m2(4, 1) + - this->data[3][5] * _m2(5, 1), - - this->data[3][0] * _m2(0, 2) + - this->data[3][1] * _m2(1, 2) + - this->data[3][2] * _m2(2, 2) + - this->data[3][3] * _m2(3, 2) + - this->data[3][4] * _m2(4, 2) + - this->data[3][5] * _m2(5, 2), - - this->data[3][0] * _m2(0, 3) + - this->data[3][1] * _m2(1, 3) + - this->data[3][2] * _m2(2, 3) + - this->data[3][3] * _m2(3, 3) + - this->data[3][4] * _m2(4, 3) + - this->data[3][5] * _m2(5, 3), - - this->data[3][0] * _m2(0, 4) + - this->data[3][1] * _m2(1, 4) + - this->data[3][2] * _m2(2, 4) + - this->data[3][3] * _m2(3, 4) + - this->data[3][4] * _m2(4, 4) + - this->data[3][5] * _m2(5, 4), - - this->data[3][0] * _m2(0, 5) + - this->data[3][1] * _m2(1, 5) + - this->data[3][2] * _m2(2, 5) + - this->data[3][3] * _m2(3, 5) + - this->data[3][4] * _m2(4, 5) + - this->data[3][5] * _m2(5, 5), - - this->data[4][0] * _m2(0, 0) + - this->data[4][1] * _m2(1, 0) + - this->data[4][2] * _m2(2, 0) + - this->data[4][3] * _m2(3, 0) + - this->data[4][4] * _m2(4, 0) + - this->data[4][5] * _m2(5, 0), - - this->data[4][0] * _m2(0, 1) + - this->data[4][1] * _m2(1, 1) + - this->data[4][2] * _m2(2, 1) + - this->data[4][3] * _m2(3, 1) + - this->data[4][4] * _m2(4, 1) + - this->data[4][5] * _m2(5, 1), - - this->data[4][0] * _m2(0, 2) + - this->data[4][1] * _m2(1, 2) + - this->data[4][2] * _m2(2, 2) + - this->data[4][3] * _m2(3, 2) + - this->data[4][4] * _m2(4, 2) + - this->data[4][5] * _m2(5, 2), - - this->data[4][0] * _m2(0, 3) + - this->data[4][1] * _m2(1, 3) + - this->data[4][2] * _m2(2, 3) + - this->data[4][3] * _m2(3, 3) + - this->data[4][4] * _m2(4, 3) + - this->data[4][5] * _m2(5, 3), - - this->data[4][0] * _m2(0, 4) + - this->data[4][1] * _m2(1, 4) + - this->data[4][2] * _m2(2, 4) + - this->data[4][3] * _m2(3, 4) + - this->data[4][4] * _m2(4, 4) + - this->data[4][5] * _m2(5, 4), - - this->data[4][0] * _m2(0, 5) + - this->data[4][1] * _m2(1, 5) + - this->data[4][2] * _m2(2, 5) + - this->data[4][3] * _m2(3, 5) + - this->data[4][4] * _m2(4, 5) + - this->data[4][5] * _m2(5, 5), - - this->data[5][0] * _m2(0, 0) + - this->data[5][1] * _m2(1, 0) + - this->data[5][2] * _m2(2, 0) + - this->data[5][3] * _m2(3, 0) + - this->data[5][4] * _m2(4, 0) + - this->data[5][5] * _m2(5, 0), - - this->data[5][0] * _m2(0, 1) + - this->data[5][1] * _m2(1, 1) + - this->data[5][2] * _m2(2, 1) + - this->data[5][3] * _m2(3, 1) + - this->data[5][4] * _m2(4, 1) + - this->data[5][5] * _m2(5, 1), - - this->data[5][0] * _m2(0, 2) + - this->data[5][1] * _m2(1, 2) + - this->data[5][2] * _m2(2, 2) + - this->data[5][3] * _m2(3, 2) + - this->data[5][4] * _m2(4, 2) + - this->data[5][5] * _m2(5, 2), - - this->data[5][0] * _m2(0, 3) + - this->data[5][1] * _m2(1, 3) + - this->data[5][2] * _m2(2, 3) + - this->data[5][3] * _m2(3, 3) + - this->data[5][4] * _m2(4, 3) + - this->data[5][5] * _m2(5, 3), - - this->data[5][0] * _m2(0, 4) + - this->data[5][1] * _m2(1, 4) + - this->data[5][2] * _m2(2, 4) + - this->data[5][3] * _m2(3, 4) + - this->data[5][4] * _m2(4, 4) + - this->data[5][5] * _m2(5, 4), - - this->data[5][0] * _m2(0, 5) + - this->data[5][1] * _m2(1, 5) + - this->data[5][2] * _m2(2, 5) + - this->data[5][3] * _m2(3, 5) + - this->data[5][4] * _m2(4, 5) + - this->data[5][5] * _m2(5, 5)); + 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, 3]. + /// range of [0, 5]. /// \param[in] _row the row index. Index values are clamped to a - /// range of [0, 3]. + /// range of [0, 5]. /// \return The value at the specified index public: inline const T &operator()(const size_t _row, const size_t _col) const @@ -568,12 +330,12 @@ namespace ignition clamp(_col, IGN_ZERO_SIZE_T, IGN_FIVE_SIZE_T)]; } - /// \brief Get a mutable version the value at the specified row, + /// \brief Get a mutable version of the value at the specified row, /// column index /// \param[in] _row the row index. Index values are clamped to a - /// range of [0, 3]. + /// range of [0, 5]. /// \param[in] _col The column index. Index values are clamped to a - /// range of [0, 3]. + /// range of [0, 5]. /// \return The value at the specified index public: inline T &operator()(const size_t _row, const size_t _col) { @@ -582,7 +344,7 @@ namespace ignition } /// \brief Get one of the four 3x3 submatrices that compose this matrix. - /// These submatrices are formed by dividing the 6x6 matrix in 4 parts and + /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that /// do not overlap with each other. /// \param[in] _corner Which corner to retrieve. /// \return A new matrix containing the values of the submatrix. @@ -609,6 +371,32 @@ namespace ignition this->data[row + 2][col + 2]}; } + /// \brief Set one of the four 3x3 submatrices that compose this matrix. + /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that + /// do not overlap with each other. + /// \param[in] _corner Which corner to set. + /// \param[in] The matrix to set. + public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3 &_mat) + { + size_t row = 0; + size_t col = 0; + if (_corner == BOTTOM_LEFT || _corner == BOTTOM_RIGHT) + { + row = 3; + } + if (_corner == TOP_RIGHT || _corner == BOTTOM_RIGHT) + { + col = 3; + } + for (size_t r = 0; r < 3; ++r) + { + for (size_t c = 0; c < 3; ++c) + { + this->data[row + r][col + c] = _mat(r, c); + } + } + } + /// \brief Equality test with tolerance. /// \param[in] _m the matrix to compare to /// \param[in] _tol equality tolerance. @@ -655,7 +443,7 @@ namespace ignition } /// \brief Equality operator - /// \param[in] _m Matrix3 to test + /// \param[in] _m Matrix6 to test /// \return true if the 2 matrices are equal (using the tolerance 1e-6), /// false otherwise public: bool operator==(const Matrix6 &_m) const diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index 359f8493d..e134b425d 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -307,3 +307,37 @@ TEST(Matrix6dTest, Submatrix) 33, 34, 35)); } +///////////////////////////////////////////////// +TEST(Matrix6dTest, SetSubmatrix) +{ + Matrix6i mat; + + mat.SetSubmatrix(Matrix6i::TOP_LEFT, Matrix3i( + 0, 1, 2, + 6, 7, 8, + 12, 13, 14)); + + mat.SetSubmatrix(Matrix6i::TOP_RIGHT, Matrix3i( + 3, 4, 5, + 9, 10, 11, + 15, 16, 17)); + + mat.SetSubmatrix(Matrix6i::BOTTOM_LEFT, Matrix3i( + 18, 19, 20, + 24, 25, 26, + 30, 31, 32)); + + mat.SetSubmatrix(Matrix6i::BOTTOM_RIGHT, Matrix3i( + 21, 22, 23, + 27, 28, 29, + 33, 34, 35)); + + EXPECT_EQ(mat, Matrix6i( + 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)); +} + diff --git a/src/python_pybind11/src/Matrix6.hh b/src/python_pybind11/src/Matrix6.hh index 1785adf7f..fb6456253 100644 --- a/src/python_pybind11/src/Matrix6.hh +++ b/src/python_pybind11/src/Matrix6.hh @@ -82,6 +82,9 @@ void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr) .def("submatrix", &Class::Submatrix, "One of the four 3x3 submatrices that compose this matrix.") + .def("set_submatrix", + &Class::SetSubmatrix, + "Set one of the four 3x3 submatrices that compose this matrix.") .def("__copy__", [](const Class &_self) { return Class(_self); }) diff --git a/src/python_pybind11/test/Matrix6_TEST.py b/src/python_pybind11/test/Matrix6_TEST.py index f307253ae..718c196d7 100644 --- a/src/python_pybind11/test/Matrix6_TEST.py +++ b/src/python_pybind11/test/Matrix6_TEST.py @@ -199,5 +199,36 @@ def test_submatrix(self): 27, 28, 29, 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)); + + mat.set_submatrix(Matrix6dCorner.TOP_RIGHT, Matrix3d( + 3, 4, 5, + 9, 10, 11, + 15, 16, 17)); + + mat.set_submatrix(Matrix6dCorner.BOTTOM_LEFT, Matrix3d( + 18, 19, 20, + 24, 25, 26, + 30, 31, 32)); + + mat.set_submatrix(Matrix6dCorner.BOTTOM_RIGHT, Matrix3d( + 21, 22, 23, + 27, 28, 29, + 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)); + if __name__ == '__main__': unittest.main() From f12eedc37f91aac95073c31d8374f4a343e6daeb Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 Jul 2022 10:11:00 -0700 Subject: [PATCH 08/10] doxy Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 67a0320ec..137ef2675 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -375,7 +375,7 @@ namespace ignition /// These submatrices are formed by dividing the 6x6 matrix in 4 parts that /// do not overlap with each other. /// \param[in] _corner Which corner to set. - /// \param[in] The matrix to set. + /// \param[in] _mat The matrix to set. public: void SetSubmatrix(Matrix6Corner _corner, const Matrix3 &_mat) { size_t row = 0; From 030e2c12d097c34462b9eff002d73a78fb68ddc5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 7 Jul 2022 11:13:38 -0700 Subject: [PATCH 09/10] eigen conversion Signed-off-by: Louise Poubel --- .../ignition/math/eigen3/Conversions.hh | 39 ++++++++++++++++++ eigen3/src/Conversions_TEST.cc | 40 +++++++++++++++++++ 2 files changed, 79 insertions(+) diff --git a/eigen3/include/ignition/math/eigen3/Conversions.hh b/eigen3/include/ignition/math/eigen3/Conversions.hh index 2dcd1b09f..42e012250 100644 --- a/eigen3/include/ignition/math/eigen3/Conversions.hh +++ b/eigen3/include/ignition/math/eigen3/Conversions.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -66,6 +67,25 @@ namespace ignition return matrix; } + /// \brief Convert from ignition::math::Matrix6d to Eigen::Matrix. + /// \param[in] _m ignition::math::Matrix6d to convert. + /// \return The equivalent Eigen::Matrix. + /// \tparam Precision Precision such as int, double or float. + template + inline Eigen::Matrix convert(const Matrix6 &_m) + { + Eigen::Matrix matrix; + for (std::size_t i = 0; i < 6; ++i) + { + for (std::size_t j = 0; j < 6; ++j) + { + matrix(i, j) = _m(i, j); + } + } + + return matrix; + } + /// \brief Convert ignition::math::Quaterniond to Eigen::Quaterniond. /// \param[in] _q ignition::math::Quaterniond to convert. /// \return The equivalent Eigen::Quaterniond. @@ -135,6 +155,25 @@ namespace ignition return matrix; } + /// \brief Convert Eigen::Matrix to ignition::math::Matrix6d. + /// \param[in] _m Eigen::Matrix to convert. + /// \return The equivalent ignition::math::Matrix6d. + /// \tparam Precision Precision such as int, double or float. + template + inline Matrix6 convert(const Eigen::Matrix &_m) + { + Matrix6 matrix; + for (std::size_t i = 0; i < 6; ++i) + { + for (std::size_t j = 0; j < 6; ++j) + { + matrix(i, j) = _m(i, j); + } + } + + return matrix; + } + /// \brief Convert Eigen::Quaterniond to ignition::math::Quaterniond. /// \param[in] _q Eigen::Quaterniond to convert. /// \return The equivalent ignition::math::Quaterniond. diff --git a/eigen3/src/Conversions_TEST.cc b/eigen3/src/Conversions_TEST.cc index 73e667193..628311407 100644 --- a/eigen3/src/Conversions_TEST.cc +++ b/eigen3/src/Conversions_TEST.cc @@ -144,6 +144,46 @@ TEST(EigenConversions, ConvertMatrix3) } } +///////////////////////////////////////////////// +/// Check Matrix6 conversions +TEST(EigenConversions, ConvertMatrix6) +{ + { + ignition::math::Matrix6d iMat, iMat2; + auto eMat = ignition::math::eigen3::convert(iMat); + for (std::size_t i = 0; i < 6; ++i) + { + for (std::size_t j = 0; j < 6; ++j) + { + EXPECT_DOUBLE_EQ(0.0, eMat(i, j)); + } + } + iMat2 = ignition::math::eigen3::convert(eMat); + EXPECT_EQ(iMat, iMat2); + } + + { + ignition::math::Matrix6d iMat( + 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); + ignition::math::Matrix6d iMat2; + auto eMat = ignition::math::eigen3::convert(iMat); + for (std::size_t i = 0; i < 6; ++i) + { + for (std::size_t j = 0; j < 6; ++j) + { + EXPECT_DOUBLE_EQ(iMat(i, j), eMat(i, j)); + } + } + iMat2 = ignition::math::eigen3::convert(eMat); + EXPECT_EQ(iMat, iMat2); + } +} + ///////////////////////////////////////////////// /// Check Pose conversions TEST(EigenConversions, ConvertPose3) From 8e99bdf4765edddbcc433b0cedf18de9f9cdd3d7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 7 Jul 2022 11:35:59 -0700 Subject: [PATCH 10/10] Fix line length Signed-off-by: Steve Peters --- eigen3/include/ignition/math/eigen3/Conversions.hh | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/eigen3/include/ignition/math/eigen3/Conversions.hh b/eigen3/include/ignition/math/eigen3/Conversions.hh index 42e012250..dc7334f68 100644 --- a/eigen3/include/ignition/math/eigen3/Conversions.hh +++ b/eigen3/include/ignition/math/eigen3/Conversions.hh @@ -67,12 +67,14 @@ namespace ignition return matrix; } - /// \brief Convert from ignition::math::Matrix6d to Eigen::Matrix. + /// \brief Convert from ignition::math::Matrix6d to + /// Eigen::Matrix. /// \param[in] _m ignition::math::Matrix6d to convert. /// \return The equivalent Eigen::Matrix. /// \tparam Precision Precision such as int, double or float. template - inline Eigen::Matrix convert(const Matrix6 &_m) + inline + Eigen::Matrix convert(const Matrix6 &_m) { Eigen::Matrix matrix; for (std::size_t i = 0; i < 6; ++i) @@ -155,12 +157,14 @@ namespace ignition return matrix; } - /// \brief Convert Eigen::Matrix to ignition::math::Matrix6d. + /// \brief Convert Eigen::Matrix to + /// ignition::math::Matrix6d. /// \param[in] _m Eigen::Matrix to convert. /// \return The equivalent ignition::math::Matrix6d. /// \tparam Precision Precision such as int, double or float. template - inline Matrix6 convert(const Eigen::Matrix &_m) + inline + Matrix6 convert(const Eigen::Matrix &_m) { Matrix6 matrix; for (std::size_t i = 0; i < 6; ++i)