diff --git a/Changelog.md b/Changelog.md index 1ce976fef..feccc74b8 100644 --- a/Changelog.md +++ b/Changelog.md @@ -3,7 +3,8 @@ ### Ignition Math 7.0.x 1. Updates per issue #101 - * [Pull request 327](https://bitbucket.org/ignitionrobotics/ign-math/pull-requests/327) + * Quaternion: [Pull request 327](https://bitbucket.org/ignitionrobotics/ign-math/pull-requests/327) + * Matrix3: [Pull request 328](https://bitbucket.org/ignitionrobotics/ign-math/pull-requests/328) 1. Removed deprecations. * [Pull request 320](https://bitbucket.org/ignitionrobotics/ign-math/pull-requests/320) diff --git a/Migration.md b/Migration.md index acf94518d..d82e09b11 100644 --- a/Migration.md +++ b/Migration.md @@ -9,6 +9,17 @@ release will remove the deprecated code. ### Deprecations +1. **Matrix3.hh** + + ***Deprecation:*** public: void Axes(const Vector3 &, const Vector3 &, const Vector3 &) + + ***Replacement:*** public: void SetAxes(const Vector3 &, const Vector3 &, const Vector3 &) + + + ***Deprecation:*** public: void Axis(const Vector3 &, T) + + ***Replacement:*** public: void SetFromAxisAngle(const Vector3 &, T) + + ***Deprecation:*** public: void From2Axes(const Vector3 &, const Vector3 &) + + ***Replacement:*** public: void SetFrom2Axes(const Vector3 &, const Vector3 &) + + ***Deprecation:*** public: void Col(unsigned int, const Vector3 &) + + ***Replacement:*** public: void SetCol(unsigned int, const Vector3 &) + 1. **Quaternion.hh** + ***Deprecation:*** public: void Axis(T, T, T, T) + ***Replacement:*** public: void SetFromAxisAngle(T, T, T, T) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index fac43fc6e..2b9e1ac63 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,20 +4,23 @@ cmake_minimum_required(VERSION 3.5 FATAL_ERROR) set(IGN_MATH_VER 7) find_package(ignition-math${IGN_MATH_VER} REQUIRED) -add_executable(vector2_example vector2_example.cc) -target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(graph_example graph_example.cc) +target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) -add_executable(triangle_example triangle_example.cc) -target_link_libraries(triangle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(matrix3_example matrix3_example.cc) +target_link_libraries(matrix3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) add_executable(quaternion_example quaternion_example.cc) target_link_libraries(quaternion_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(quaternion_from_euler quaternion_from_euler.cc) +target_link_libraries(quaternion_from_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(quaternion_to_euler quaternion_to_euler.cc) target_link_libraries(quaternion_to_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) -add_executable(quaternion_from_euler quaternion_from_euler.cc) -target_link_libraries(quaternion_from_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(triangle_example triangle_example.cc) +target_link_libraries(triangle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) -add_executable(graph_example graph_example.cc) -target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(vector2_example vector2_example.cc) +target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/matrix3_example.cc b/examples/matrix3_example.cc new file mode 100644 index 000000000..a3f7d3168 --- /dev/null +++ b/examples/matrix3_example.cc @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2019 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. + * +*/ +//! [complete] +#include +#include + +int main(int argc, char **argv) +{ + // Construct a default matrix3. + ignition::math::Matrix3d m; + std::cout << "The default constructed matrix m has the following values.\n\t" + << m << std::endl; + + // Set the first column of the matrix. + m.SetCol(0, ignition::math::Vector3d(3, 4, 5)); + std::cout << "Setting the first column of the matrix m to 3, 4, 5.\n\t" + << m << std::endl; + + // Transpose the matrix. + ignition::math::Matrix3d t = m.Transposed(); + std::cout << "The transposed matrix t has the values.\n\t" + << t << std::endl; + + // Multiply the two matrices. + std::cout << "m * t = " << m * t << std::endl; +} +//! [complete] diff --git a/examples/matrix3_example.rb b/examples/matrix3_example.rb new file mode 100644 index 000000000..977473c36 --- /dev/null +++ b/examples/matrix3_example.rb @@ -0,0 +1,55 @@ +# Copyright (C) 2019 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. + +# This example will only work if the Ruby interface library was compiled and +# installed. +# +# Modify the RUBYLIB environment variable to include the ignition math +# library install path. For example, if you install to /user: +# +# $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB +# +require 'ignition/math' + +# Construct a default matrix3. +m = Ignition::Math::Matrix3d.new +printf("The default constructed matrix m has the following values.\n\t" + + "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + m.(0, 0), m.(0, 1), m.(0, 2), + m.(1, 0), m.(1, 1), m.(1, 2), + m.(2, 0), m.(2, 1), m.(2, 2)) + +# Set the first column of the matrix. +m.SetCol(0, Ignition::Math::Vector3d.new(3, 4, 5)) +printf("Setting the first column of the matrix m to 3, 4, 5.\n\t" + + "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + m.(0, 0), m.(0, 1), m.(0, 2), + m.(1, 0), m.(1, 1), m.(1, 2), + m.(2, 0), m.(2, 1), m.(2, 2)) + +# Transpose the matrix. +t = m.Transposed() +printf("The transposed matrix t has the values.\n\t"+ + "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + t.(0, 0), t.(0, 1), t.(0, 2), + t.(1, 0), t.(1, 1), t.(1, 2), + t.(2, 0), t.(2, 1), t.(2, 2)) + +# Multiply the two matrices. +m = m * t +printf("m * t = " + + "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + m.(0, 0), m.(0, 1), m.(0, 2), + m.(1, 0), m.(1, 1), m.(1, 2), + m.(2, 0), m.(2, 1), m.(2, 2)) diff --git a/include/ignition/math/Matrix3.hh b/include/ignition/math/Matrix3.hh index beebec9b8..6f38110f3 100644 --- a/include/ignition/math/Matrix3.hh +++ b/include/ignition/math/Matrix3.hh @@ -35,30 +35,45 @@ namespace ignition template class Quaternion; /// \class Matrix3 Matrix3.hh ignition/math/Matrix3.hh - /// \brief A 3x3 matrix class + /// \brief A 3x3 matrix class. + /// + /// The following two type definitions are provided: + /// + /// * \ref Matrix3i : Equivalent to Matrix3 + /// * \ref Matrix3f : Equivalent to Matrix3 + /// * \ref Matrix3d : Equivalent to Matrix3 + /// ## Examples + /// + /// * C++ + /// + /// \snippet examples/quaternion_example.cc complete + /// + /// * Ruby template class Matrix3 { - /// \brief Identity matrix + /// \brief A Matrix3 initialized to identity. + /// This is equivaledn to math::Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1). public: static const Matrix3 Identity; - /// \brief Zero matrix + /// \brief A Matrix3 initialized to zero. + /// This is equivaledn to math::Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0). public: static const Matrix3 Zero; - /// \brief Constructor + /// \brief Default constructor that initializes the matrix3 to zero. public: Matrix3() { std::memset(this->data, 0, sizeof(this->data[0][0])*9); } - /// \brief Copy constructor + /// \brief Copy constructor. /// \param _m Matrix to copy public: Matrix3(const Matrix3 &_m) { std::memcpy(this->data, _m.data, sizeof(this->data[0][0])*9); } - /// \brief Constructor + /// \brief Construct a matrix3 using nine 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 @@ -84,7 +99,7 @@ namespace ignition } /// \brief Construct Matrix3 from a quaternion. - /// \param[in] _q Quaternion. + /// \param[in] _q Quaternion to set the Matrix3 from. public: explicit Matrix3(const Quaternion &_q) { Quaternion qt = _q; @@ -101,9 +116,19 @@ namespace ignition } /// \brief Desctructor - public: virtual ~Matrix3() {} + public: ~Matrix3() {} - /// \brief Set values + /// \brief Set a single value. + /// \param[in] _row row index. _row is clamped to the range [0,2] + /// \param[in] _col column index. _col is clamped to the range [0,2] + /// \param[in] _v New value. + public: void Set(size_t _row, size_t _col, T _v) + { + this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] = _v; + } + + /// \brief Set 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 @@ -128,23 +153,45 @@ namespace ignition this->data[2][2] = _v22; } - /// \brief Set the matrix from three axis (1 per column) - /// \param[in] _xAxis The x axis - /// \param[in] _yAxis The y axis - /// \param[in] _zAxis The z axis - public: void Axes(const Vector3 &_xAxis, - const Vector3 &_yAxis, - const Vector3 &_zAxis) + /// \brief Set the matrix from three axis (1 per column). + /// \param[in] _xAxis The x axis, the first column of the matrix. + /// \param[in] _yAxis The y axis, the second column of the matrix. + /// \param[in] _zAxis The z axis, the third column of the matrix. + /// \deprecated Use SetAxes(const Vector3 &, const Vector3 &, + /// const Vector3 &,) + public: void IGN_DEPRECATED(7) Axes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis) { - this->Col(0, _xAxis); - this->Col(1, _yAxis); - this->Col(2, _zAxis); + this->SetAxes(_xAxis, _yAxis, _zAxis); } - /// \brief Set the matrix from an axis and angle + /// \brief Set the matrix from three axis (1 per column). + /// \param[in] _xAxis The x axis, the first column of the matrix. + /// \param[in] _yAxis The y axis, the second column of the matrix. + /// \param[in] _zAxis The z axis, the third column of the matrix. + public: void SetAxes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis) + { + this->SetCol(0, _xAxis); + this->SetCol(1, _yAxis); + this->SetCol(2, _zAxis); + } + + /// \brief Set the matrix from an axis and angle. /// \param[in] _axis the axis /// \param[in] _angle ccw rotation around the axis in radians - public: void Axis(const Vector3 &_axis, T _angle) + /// \deprecated Use SetFromAxisAngle(const Vector3 &, T) + public: void IGN_DEPRECATED(7) Axis(const Vector3 &_axis, T _angle) + { + this->SetFromAxisAngle(_axis, _angle); + } + + /// \brief Set the matrix from an axis and angle. + /// \param[in] _axis the axis + /// \param[in] _angle ccw rotation around the axis in radians + public: void SetFromAxisAngle(const Vector3 &_axis, T _angle) { T c = cos(_angle); T s = sin(_angle); @@ -169,7 +216,20 @@ namespace ignition /// /// \param[in] _v1 The first vector /// \param[in] _v2 The second vector - public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2) + /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) + public: void IGN_DEPRECATED(7) From2Axes( + const Vector3 &_v1, const Vector3 &_v2) + { + this->SetFrom2Axes(_v1, _v2); + } + + /// \brief Set the matrix to represent rotation from + /// vector _v1 to vector _v2, so that + /// _v2.Normalize() == this * _v1.Normalize() holds. + /// + /// \param[in] _v1 The first vector + /// \param[in] _v2 The second vector + public: void SetFrom2Axes(const Vector3 &_v1, const Vector3 &_v2) { const T _v1LengthSquared = _v1.SquaredLength(); if (_v1LengthSquared <= 0.0) @@ -203,14 +263,24 @@ namespace ignition const Vector3 cross = _v1.Cross(_v2).Normalize(); - this->Axis(cross, acos(dot)); + this->SetFromAxisAngle(cross, acos(dot)); + } + + /// \brief Set a column. + /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the + /// range [0, 2]. + /// \param[in] _v The value to set in each row of the column. + /// \deprecated Use SetCol(unsigned int _c, const Vector3 &_v) + public: void IGN_DEPRECATED(7) Col(unsigned int _c, const Vector3 &_v) + { + this->SetCol(_c, _v); } /// \brief Set a column. /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the /// range [0, 2]. /// \param[in] _v The value to set in each row of the column. - public: void Col(unsigned int _c, const Vector3 &_v) + public: void SetCol(unsigned int _c, const Vector3 &_v) { unsigned int c = clamp(_c, 0u, 2u); @@ -220,15 +290,17 @@ namespace ignition } /// \brief Equal operator. this = _mat - /// \param _mat Incoming matrix - /// \return itself + /// \param _mat Matrix to copy. + /// \return This matrix. public: Matrix3 &operator=(const Matrix3 &_mat) { memcpy(this->data, _mat.data, sizeof(this->data[0][0])*9); return *this; } - /// \brief returns the element wise difference of two matrices + /// \brief Subtraction operator. + /// \param[in] _m Matrix to subtract. + /// \return The element wise difference of two matrices. public: Matrix3 operator-(const Matrix3 &_m) const { return Matrix3( @@ -243,7 +315,9 @@ namespace ignition this->data[2][2] - _m(2, 2)); } - /// \brief returns the element wise sum of two matrices + /// \brief Addition operation. + /// \param[in] _m Matrix to add. + /// \return The element wise sum of two matrices public: Matrix3 operator+(const Matrix3 &_m) const { return Matrix3( @@ -258,7 +332,9 @@ namespace ignition this->data[2][2]+_m(2, 2)); } - /// \brief returns the element wise scalar multiplication + /// \brief Scalar multiplication operator. + /// \param[in] _s Value to multiply. + /// \return The element wise scalar multiplication. public: Matrix3 operator*(const T &_s) const { return Matrix3( @@ -269,7 +345,7 @@ namespace ignition /// \brief Matrix multiplication operator /// \param[in] _m Matrix3 to multiply - /// \return product of this * _m + /// \return Product of this * _m public: Matrix3 operator*(const Matrix3 &_m) const { return Matrix3( @@ -370,15 +446,15 @@ namespace ignition && equal(this->data[2][2], _m(2, 2), _tol); } - /// \brief Equality test operator - /// \param[in] _m Matrix3 to test - /// \return True if equal (using the default tolerance of 1e-6) + /// \brief Equality test operator. + /// \param[in] _m Matrix3 to test. + /// \return True if equal (using the default tolerance of 1e-6). public: bool operator==(const Matrix3 &_m) const { return this->Equal(_m, static_cast(1e-6)); } - /// \brief Set the matrix3 from a quaternion + /// \brief Set the matrix3 from a quaternion. /// \param[in] _q Quaternion to set the matrix3 from. /// \return Reference to the new matrix3 object. public: Matrix3 &operator=(const Quaternion &_q) @@ -386,25 +462,25 @@ namespace ignition return *this = Matrix3(_q); } - /// \brief Inequality test operator - /// \param[in] _m Matrix3 to test - /// \return True if not equal (using the default tolerance of 1e-6) + /// \brief Inequality test operator. + /// \param[in] _m Matrix3 to test. + /// \return True if not equal (using the default tolerance of 1e-6). public: bool operator!=(const Matrix3 &_m) const { return !(*this == _m); } - /// \brief Array subscript operator + /// \brief Array subscript operator. /// \param[in] _row row index. _row is clamped to the range [0,2] /// \param[in] _col column index. _col is clamped to the range [0,2] /// \return a pointer to the row - public: inline const T &operator()(size_t _row, size_t _col) const + public: inline T operator()(size_t _row, size_t _col) const { return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; } - /// \brief Array subscript operator + /// \brief Array subscript operator. /// \param[in] _row row index. _row is clamped to the range [0,2] /// \param[in] _col column index. _col is clamped to the range [0,2] /// \return a pointer to the row @@ -414,7 +490,7 @@ namespace ignition [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; } - /// \brief Return the determinant of the matrix + /// \brief Return the determinant of the matrix. /// \return Determinant of this matrix. public: T Determinant() const { @@ -432,7 +508,7 @@ namespace ignition + t2 * this->data[0][2]; } - /// \brief Return the inverse matrix + /// \brief Return the inverse matrix. /// \return Inverse of this matrix. public: Matrix3 Inverse() const { @@ -475,7 +551,7 @@ namespace ignition std::swap(this->data[1][2], this->data[2][1]); } - /// \brief Return the transpose of this matrix + /// \brief Return the transpose of this matrix. /// \return Transpose of this matrix. public: Matrix3 Transposed() const { @@ -485,10 +561,11 @@ namespace ignition this->data[0][2], this->data[1][2], this->data[2][2]); } - /// \brief Stream insertion operator - /// \param[in] _out Output stream - /// \param[in] _m Matrix to output - /// \return the stream + /// \brief Stream insertion operator. This operator outputs all + /// 9 scalar values in the matrix separated by a single space. + /// \param[in, out] _out Output stream. + /// \param[in] _m Matrix to output. + /// \return The stream. public: friend std::ostream &operator<<( std::ostream &_out, const ignition::math::Matrix3 &_m) { @@ -504,10 +581,12 @@ namespace ignition return _out; } - /// \brief Stream extraction operator - /// \param _in input stream - /// \param _pt Matrix3 to read values into - /// \return the stream + + /// \brief Stream extraction operator. This operator requires 9 space + /// separated scalar values, such as "1 2 3 4 5 6 7 8 9". + /// \param [in, out] _in Input stream. + /// \param [out] _pt Matrix3 to read values into. + /// \return The stream. public: friend std::istream &operator>>( std::istream &_in, ignition::math::Matrix3 &_m) { @@ -540,11 +619,15 @@ namespace ignition 0, 0, 0, 0, 0, 0); + /// \typedef Matrix3 Matrix3i typedef Matrix3 Matrix3i; + + /// \typedef Matrix3 Matrix3d typedef Matrix3 Matrix3d; + + /// \typedef Matrix3 Matrix3f typedef Matrix3 Matrix3f; } } } - #endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 59b4d9c40..84a66a823 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,7 @@ if (SWIG_FOUND) set(swig_files ruby Angle + Matrix3 Quaternion Vector2 Vector3) diff --git a/src/Matrix3.i b/src/Matrix3.i new file mode 100644 index 000000000..f587bc4e7 --- /dev/null +++ b/src/Matrix3.i @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2019 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. + * +*/ +#ifdef SWIGRUBY +%begin %{ +#define HAVE_ISFINITE 1 +%} +#endif + +%module matrix3 +%{ +#include +%} + +namespace ignition +{ + namespace math + { + template + class Matrix3 + { + public: static const Matrix3 Identity; + public: static const Matrix3 Zero; + public: Matrix3(); + public: Matrix3(const Matrix3 &_m); + public: Matrix3(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: explicit Matrix3(const ignition::math::Quaternion &_q); + public: ~Matrix3(); + public: void Set(size_t _row, size_t _col, T _v); + public: void Set(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22); + public: void SetAxes(const ignition::math::Vector3 &_xAxis, + const ignition::math::Vector3 &_yAxis, + const ignition::math::Vector3 &_zAxis); + public: void SetFromAxisAngle(const ignition::math::Vector3 &_axis, + T _angle); + public: void SetFrom2Axes(const ignition::math::Vector3 &_v1, + const ignition::math::Vector3 &_v2); + public: void SetCol(unsigned int _c, + const ignition::math::Vector3 &_v); + public: Matrix3 operator-(const Matrix3 &_m) const; + public: Matrix3 operator+(const Matrix3 &_m) const; + public: Matrix3 operator*(const T &_s) const; + public: Matrix3 operator*(const Matrix3 &_m) const; + public: ignition::math::Vector3 operator*( + const ignition::math::Vector3 &_vec) const; + public: bool Equal(const Matrix3 &_m, const T &_tol) const; + public: bool operator==(const Matrix3 &_m) const; + public: inline T operator()(size_t _row, size_t _col) const; + public: T Determinant() const; + public: Matrix3 Inverse() const; + public: void Transpose(); + public: Matrix3 Transposed() const; + }; + + %template(Matrix3i) Matrix3; + %template(Matrix3d) Matrix3; + %template(Matrix3f) Matrix3; + } +} diff --git a/src/Matrix3_TEST.cc b/src/Matrix3_TEST.cc index ddcdbf6d6..4f11340b4 100644 --- a/src/Matrix3_TEST.cc +++ b/src/Matrix3_TEST.cc @@ -28,6 +28,12 @@ TEST(Matrix3dTest, Matrix3d) { math::Matrix3d matrix; EXPECT_TRUE(matrix == math::Matrix3d(0, 0, 0, 0, 0, 0, 0, 0, 0)); + matrix.Set(0, 0, 1); + EXPECT_TRUE(matrix == math::Matrix3d(1, 0, 0, 0, 0, 0, 0, 0, 0)); + matrix.Set(5, 0, 2); + EXPECT_TRUE(matrix == math::Matrix3d(1, 0, 0, 0, 0, 0, 2, 0, 0)); + matrix.Set(5, 5, 3); + EXPECT_TRUE(matrix == math::Matrix3d(1, 0, 0, 0, 0, 0, 2, 0, 3)); } { @@ -39,17 +45,17 @@ TEST(Matrix3dTest, Matrix3d) } math::Matrix3d matrix; - matrix.Axes(math::Vector3d(1, 1, 1), math::Vector3d(2, 2, 2), + matrix.SetAxes(math::Vector3d(1, 1, 1), math::Vector3d(2, 2, 2), math::Vector3d(3, 3, 3)); EXPECT_TRUE(matrix == math::Matrix3d(1, 2, 3, 1, 2, 3, 1, 2, 3)); - matrix.Axis(math::Vector3d(1, 1, 1), IGN_PI); + matrix.SetFromAxisAngle(math::Vector3d(1, 1, 1), IGN_PI); EXPECT_TRUE(matrix == math::Matrix3d(1, 2, 2, 2, 1, 2, 2, 2, 1)); - matrix.Col(0, math::Vector3d(3, 4, 5)); + matrix.SetCol(0, math::Vector3d(3, 4, 5)); EXPECT_TRUE(matrix == math::Matrix3d(3, 2, 2, 4, 1, 2, 5, 2, 1)); - EXPECT_NO_THROW(matrix.Col(3, math::Vector3d(1, 1, 1))); + EXPECT_NO_THROW(matrix.SetCol(3, math::Vector3d(1, 1, 1))); EXPECT_TRUE(matrix == math::Matrix3d(3, 2, 1, 4, 1, 1, 5, 2, 1)); } @@ -335,10 +341,10 @@ TEST(Matrix3dTest, From2Axes) math::Vector3d v2(0.0, 1.0, 0.0); math::Matrix3d m1; - m1.From2Axes(v1, v2); + m1.SetFrom2Axes(v1, v2); math::Matrix3d m2; - m2.From2Axes(v2, v1); + m2.SetFrom2Axes(v2, v1); math::Matrix3d m1Correct(0, -1, 0, 1, 0, 0, @@ -356,7 +362,7 @@ TEST(Matrix3dTest, From2Axes) // rotation about 45 degrees v1.Set(1.0, 0.0, 0.0); v2.Set(1.0, 1.0, 0.0); - m2.From2Axes(v1, v2); + m2.SetFrom2Axes(v1, v2); // m1 is 90 degrees rotation EXPECT_EQ(m1, m2*m2); @@ -364,8 +370,8 @@ TEST(Matrix3dTest, From2Axes) v1.Set(0.5, 0.5, 0); v2.Set(-0.5, 0.5, 0); - m1.From2Axes(v1, v2); - m2.From2Axes(v2, v1); + m1.SetFrom2Axes(v1, v2); + m2.SetFrom2Axes(v2, v1); EXPECT_NE(m1, m2); EXPECT_EQ(m1Correct, m1); @@ -377,24 +383,24 @@ TEST(Matrix3dTest, From2Axes) // For zero-length vectors, a unit matrix is returned v1.Set(0, 0, 0); v2.Set(-0.5, 0.5, 0); - m1.From2Axes(v1, v2); + m1.SetFrom2Axes(v1, v2); EXPECT_EQ(math::Matrix3d::Identity, m1); // For zero-length vectors, a unit matrix is returned v1.Set(-0.5, 0.5, 0); v2.Set(0, 0, 0); - m1.From2Axes(v1, v2); + m1.SetFrom2Axes(v1, v2); EXPECT_EQ(math::Matrix3d::Identity, m1); // Parallel vectors v1.Set(1, 0, 0); v2.Set(2, 0, 0); - m1.From2Axes(v1, v2); + m1.SetFrom2Axes(v1, v2); EXPECT_EQ(math::Matrix3d::Identity, m1); // Opposite vectors v1.Set(1, 0, 0); v2.Set(-2, 0, 0); - m1.From2Axes(v1, v2); + m1.SetFrom2Axes(v1, v2); EXPECT_EQ(math::Matrix3d::Zero - math::Matrix3d::Identity, m1); } diff --git a/src/Matrix3_TEST.rb b/src/Matrix3_TEST.rb new file mode 100644 index 000000000..c88ac23ff --- /dev/null +++ b/src/Matrix3_TEST.rb @@ -0,0 +1,95 @@ +# Copyright (C) 2019 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. + +#!/usr/bin/env ruby + +require 'test/unit/ui/console/testrunner' +require 'test/unit' +require 'math' + +class Matrix3_TEST < Test::Unit::TestCase + def test_construction + m = Ignition::Math::Matrix3d.new + assert(m.(0, 0) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (0,0)==0") + assert(m.(0, 1) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (0,1)==0") + assert(m.(0, 2) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (0,2)==0") + assert(m.(1, 0) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (1,0)==0") + assert(m.(1, 1) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (1,1)==0") + assert(m.(1, 2) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (1,2)==0") + assert(m.(2, 0) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (2,0)==0") + assert(m.(2, 1) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (2,1)==0") + assert(m.(2, 2) == 0.0, + "Ignition::Math::Matrix3d default constructor should have (2,2)==0") + + m1 = Ignition::Math::Matrix3d.new(1, 2, 3, 4, 5, 6 , 7, 8, 9) + assert(m1.(0, 0) == 1.0, + "m1 should have (0,0)==1") + assert(m1.(0, 1) == 2.0, + "m1 should have (0,1)==2") + assert(m1.(0, 2) == 3.0, + "m1 should have (0,2)==3") + assert(m1.(1, 0) == 4.0, + "m1 should have (1,0)==4") + assert(m1.(1, 1) == 5.0, + "m1 should have (1,1)==5") + assert(m1.(1, 2) == 6.0, + "m1 should have (1,2)==6") + assert(m1.(2, 0) == 7.0, + "m1 should have (2,0)==7") + assert(m1.(2, 1) == 8.0, + "m1 should have (2,1)==8") + assert(m1.(2, 2) == 9.0, + "m1 should have (2,2)==9") + + mT = m1.Transposed() + puts mT.(0,0) + puts mT.(0,1) + puts mT.(0,2) + puts mT.(1,0) + puts mT.(1,1) + puts mT.(1,2) + puts mT.(2,0) + puts mT.(2,1) + puts mT.(2,2) + + assert(mT.(0, 0) == 1.0, + "mT should have (0,0)==1") + assert(mT.(0, 1) == 4.0, + "mT should have (0,1)==4") + assert(mT.(0, 2) == 7.0, + "mT should have (0,2)==7") + assert(mT.(1, 0) == 2.0, + "mT should have (1,0)==2") + assert(mT.(1, 1) == 5.0, + "mT should have (1,1)==5") + assert(mT.(1, 2) == 8.0, + "mT should have (1,2)==8") + assert(mT.(2, 0) == 3.0, + "mT should have (2,0)==3") + assert(mT.(2, 1) == 6.0, + "mT should have (2,1)==6") + assert(mT.(2, 2) == 9.0, + "mT should have (2,2)==9") + end +end + +exit Test::Unit::UI::Console::TestRunner.run(Matrix3_TEST).passed? ? 0 : -1 diff --git a/src/ign_math.i b/src/ign_math.i index d2c3f1639..26f99d0e6 100644 --- a/src/ign_math.i +++ b/src/ign_math.i @@ -1,4 +1,5 @@ %include "Angle.i" +%include "Matrix3.i" %include "Quaternion.i" %include "Vector2.i" %include "Vector3.i"