diff --git a/BUILD.bazel b/BUILD.bazel index 43d5a6bb8..3f2af6a7a 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -43,7 +43,7 @@ ign_export_header( public_headers_no_gen = glob([ "include/ignition/math/*.hh", - "include/ignition/math/detail/*.hh", + "include/gz/math/detail/*.hh", "include/ignition/math/graph/*.hh", ]) diff --git a/CMakeLists.txt b/CMakeLists.txt index 580ec379f..8116a3793 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,10 @@ find_package(ignition-cmake2 2.13.0 REQUIRED) #============================================================================ set (c++standard 17) set (CMAKE_CXX_STANDARD 17) -ign_configure_project(VERSION_SUFFIX) +ign_configure_project( + REPLACE_IGNITION_INCLUDE_PATH gz/math + VERSION_SUFFIX +) #============================================================================ # Set project-specific options diff --git a/Changelog.md b/Changelog.md index a99d12b96..722c5127b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -516,7 +516,7 @@ ### Ignition Math 6.1.0 -1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> ignition::math::AxisAlignedBox +1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> gz::math::AxisAlignedBox * [BitBucket pull request 302](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/302) ### Ignition Math 6.0.0 @@ -547,7 +547,7 @@ * [BitBucket pull request 301](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/301) * [Issue 60](https://github.com/ignitionrobotics/ign-math/issues/60) -1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> ignition::math::AxisAlignedBox +1. eigen3: add conversion functions for Eigen::AlignedBox3d <=> gz::math::AxisAlignedBox * [BitBucket pull request 302](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-math/pull-requests/302) diff --git a/eigen3/include/CMakeLists.txt b/eigen3/include/CMakeLists.txt new file mode 100644 index 000000000..4b2bdd7bb --- /dev/null +++ b/eigen3/include/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(gz) +install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}) diff --git a/include/ignition/CMakeLists.txt b/eigen3/include/gz/CMakeLists.txt similarity index 100% rename from include/ignition/CMakeLists.txt rename to eigen3/include/gz/CMakeLists.txt diff --git a/eigen3/include/ignition/math/CMakeLists.txt b/eigen3/include/gz/math/CMakeLists.txt similarity index 100% rename from eigen3/include/ignition/math/CMakeLists.txt rename to eigen3/include/gz/math/CMakeLists.txt diff --git a/eigen3/include/gz/math/eigen3/Conversions.hh b/eigen3/include/gz/math/eigen3/Conversions.hh new file mode 100644 index 000000000..bd3bbdc4e --- /dev/null +++ b/eigen3/include/gz/math/eigen3/Conversions.hh @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2018 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_EIGEN3_CONVERSIONS_HH_ +#define GZ_MATH_EIGEN3_CONVERSIONS_HH_ + +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + namespace eigen3 + { + /// \brief Convert from gz::math::Vector3d to Eigen::Vector3d. + /// \param[in] _v gz::math::Vector3d to convert + /// \return The equivalent Eigen::Vector3d. + inline Eigen::Vector3d convert(const gz::math::Vector3d &_v) + { + return Eigen::Vector3d(_v[0], _v[1], _v[2]); + } + + /// \brief Convert from gz::math::AxisAlignedBox to + /// Eigen::AlignedBox3d. + /// \param[in] _b gz::math::AxisAlignedBox to convert + /// \return The equivalent Eigen::AlignedBox3d. + inline Eigen::AlignedBox3d convert( + const gz::math::AxisAlignedBox &_b) + { + return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max())); + } + + /// \brief Convert from gz::math::Matrix3d to Eigen::Matrix3d. + /// \param[in] _m gz::math::Matrix3d to convert. + /// \return The equivalent Eigen::Matrix3d. + inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m) + { + Eigen::Matrix3d matrix; + for (std::size_t i=0; i < 3; ++i) + { + for (std::size_t j=0; j < 3; ++j) + { + matrix(i, j) = _m(i, j); + } + } + + return matrix; + } + + /// \brief Convert from gz::math::Matrix6d to + /// Eigen::Matrix. + /// \param[in] _m gz::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 gz::math::Quaterniond to Eigen::Quaterniond. + /// \param[in] _q gz::math::Quaterniond to convert. + /// \return The equivalent Eigen::Quaterniond. + inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q) + { + Eigen::Quaterniond quat; + quat.w() = _q.W(); + quat.x() = _q.X(); + quat.y() = _q.Y(); + quat.z() = _q.Z(); + + return quat; + } + + /// \brief Convert gz::math::Pose3d to Eigen::Isometry3d. + /// \param[in] _pose gz::math::Pose3d to convert. + /// \return The equivalent Eigen::Isometry3d. + inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose) + { + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = convert(_pose.Pos()); + tf.linear() = Eigen::Matrix3d(convert(_pose.Rot())); + + return tf; + } + + /// \brief Convert Eigen::Vector3d to gz::math::Vector3d. + /// \param[in] _v Eigen::Vector3d to convert. + /// \return The equivalent gz::math::Vector3d. + inline gz::math::Vector3d convert(const Eigen::Vector3d &_v) + { + gz::math::Vector3d vec; + vec.X() = _v[0]; + vec.Y() = _v[1]; + vec.Z() = _v[2]; + + return vec; + } + + /// \brief Convert Eigen::AlignedBox3d to gz::math::AxisAlignedBox. + /// \param[in] _b Eigen::AlignedBox3d to convert. + /// \return The equivalent gz::math::AxisAlignedBox. + inline gz::math::AxisAlignedBox convert( + const Eigen::AlignedBox3d &_b) + { + gz::math::AxisAlignedBox box; + box.Min() = convert(_b.min()); + box.Max() = convert(_b.max()); + + return box; + } + + /// \brief Convert Eigen::Matrix3d to gz::math::Matrix3d. + /// \param[in] _m Eigen::Matrix3d to convert. + /// \return The equivalent gz::math::Matrix3d. + inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m) + { + gz::math::Matrix3d matrix; + for (std::size_t i=0; i < 3; ++i) + { + for (std::size_t j=0; j < 3; ++j) + { + matrix(i, j) = _m(i, j); + } + } + + return matrix; + } + + /// \brief Convert Eigen::Matrix to + /// gz::math::Matrix6d. + /// \param[in] _m Eigen::Matrix to convert. + /// \return The equivalent gz::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 gz::math::Quaterniond. + /// \param[in] _q Eigen::Quaterniond to convert. + /// \return The equivalent gz::math::Quaterniond. + inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q) + { + gz::math::Quaterniond quat; + quat.W() = _q.w(); + quat.X() = _q.x(); + quat.Y() = _q.y(); + quat.Z() = _q.z(); + + return quat; + } + + /// \brief Convert Eigen::Isometry3d to gz::math::Pose3d. + /// \param[in] _tf Eigen::Isometry3d to convert. + /// \return The equivalent gz::math::Pose3d. + inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf) + { + gz::math::Pose3d pose; + pose.Pos() = convert(Eigen::Vector3d(_tf.translation())); + pose.Rot() = convert(Eigen::Quaterniond(_tf.linear())); + + return pose; + } + } + } +} + +#endif diff --git a/eigen3/include/gz/math/eigen3/Util.hh b/eigen3/include/gz/math/eigen3/Util.hh new file mode 100644 index 000000000..15f10624a --- /dev/null +++ b/eigen3/include/gz/math/eigen3/Util.hh @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2021 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_EIGEN3_UTIL_HH_ +#define GZ_MATH_EIGEN3_UTIL_HH_ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + namespace eigen3 + { + /// \brief Get covariance matrix from a set of 3d vertices + /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305 + /// \param[in] _vertices a vector of 3d vertices + /// \return Covariance matrix + inline Eigen::Matrix3d covarianceMatrix( + const std::vector &_vertices) + { + if (_vertices.empty()) + return Eigen::Matrix3d::Identity(); + + Eigen::Matrix cumulants; + cumulants.setZero(); + for (const auto &vertex : _vertices) + { + const Eigen::Vector3d &point = math::eigen3::convert(vertex); + cumulants(0) += point(0); + cumulants(1) += point(1); + cumulants(2) += point(2); + cumulants(3) += point(0) * point(0); + cumulants(4) += point(0) * point(1); + cumulants(5) += point(0) * point(2); + cumulants(6) += point(1) * point(1); + cumulants(7) += point(1) * point(2); + cumulants(8) += point(2) * point(2); + } + + Eigen::Matrix3d covariance; + + cumulants /= static_cast(_vertices.size()); + + covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); + covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1); + covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2); + covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1); + covariance(1, 0) = covariance(0, 1); + covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2); + covariance(2, 0) = covariance(0, 2); + covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2); + covariance(2, 1) = covariance(1, 2); + return covariance; + } + + /// \brief Get the oriented 3d bounding box of a set of 3d + /// vertices using PCA + /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html + /// \param[in] _vertices a vector of 3d vertices + /// \return Oriented 3D box + inline gz::math::OrientedBoxd verticesToOrientedBox( + const std::vector &_vertices) + { + math::OrientedBoxd box; + + // Return an empty box if there are no vertices + if (_vertices.empty()) + return box; + + math::Vector3d mean; + for (const auto &point : _vertices) + mean += point; + mean /= static_cast(_vertices.size()); + + Eigen::Vector3d centroid = math::eigen3::convert(mean); + Eigen::Matrix3d covariance = covarianceMatrix(_vertices); + + // Eigen Vectors + Eigen::SelfAdjointEigenSolver + eigenSolver(covariance, Eigen::ComputeEigenvectors); + Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors(); + + // This line is necessary for proper orientation in some cases. + // The numbers come out the same without it, but the signs are + // different and the box doesn't get correctly oriented in some cases. + eigenVectorsPCA.col(2) = + eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); + + // Transform the original cloud to the origin where the principal + // components correspond to the axes. + Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity()); + projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); + projectionTransform.block<3, 1>(0, 3) = + -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid); + + Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32); + Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32); + + // Get the minimum and maximum points of the transformed cloud. + for (const auto &point : _vertices) + { + Eigen::Vector4d pt(0, 0, 0, 1); + pt.head<3>() = math::eigen3::convert(point); + Eigen::Vector4d tfPoint = projectionTransform * pt; + minPoint = minPoint.cwiseMin(tfPoint.head<3>()); + maxPoint = maxPoint.cwiseMax(tfPoint.head<3>()); + } + + const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint); + + // quaternion is calculated using the eigenvectors (which determines + // how the final box gets rotated), and the transform to put the box + // in correct location is calculated + const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA); + const Eigen::Vector3d bboxTransform = + eigenVectorsPCA * meanDiagonal + centroid; + + math::Vector3d size( + maxPoint.x() - minPoint.x(), + maxPoint.y() - minPoint.y(), + maxPoint.z() - minPoint.z() + ); + math::Pose3d pose; + pose.Rot() = math::eigen3::convert(bboxQuaternion); + pose.Pos() = math::eigen3::convert(bboxTransform); + + box.Size(size); + box.Pose(pose); + return box; + } + } + } +} + +#endif diff --git a/eigen3/include/ignition/math/eigen3.hh b/eigen3/include/ignition/math/eigen3.hh new file mode 100644 index 000000000..20dafc6ec --- /dev/null +++ b/eigen3/include/ignition/math/eigen3.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/eigen3/include/ignition/math/eigen3/Conversions.hh b/eigen3/include/ignition/math/eigen3/Conversions.hh index dc7334f68..bab4a3f07 100644 --- a/eigen3/include/ignition/math/eigen3/Conversions.hh +++ b/eigen3/include/ignition/math/eigen3/Conversions.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,198 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_MATH_EIGEN3_CONVERSIONS_HH_ -#define IGNITION_MATH_EIGEN3_CONVERSIONS_HH_ - -#include -#include -#include -#include -#include -#include -#include - -namespace ignition -{ - namespace math - { - namespace eigen3 - { - /// \brief Convert from ignition::math::Vector3d to Eigen::Vector3d. - /// \param[in] _v ignition::math::Vector3d to convert - /// \return The equivalent Eigen::Vector3d. - inline Eigen::Vector3d convert(const ignition::math::Vector3d &_v) - { - return Eigen::Vector3d(_v[0], _v[1], _v[2]); - } - - /// \brief Convert from ignition::math::AxisAlignedBox to - /// Eigen::AlignedBox3d. - /// \param[in] _b ignition::math::AxisAlignedBox to convert - /// \return The equivalent Eigen::AlignedBox3d. - inline Eigen::AlignedBox3d convert( - const ignition::math::AxisAlignedBox &_b) - { - return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max())); - } - - /// \brief Convert from ignition::math::Matrix3d to Eigen::Matrix3d. - /// \param[in] _m ignition::math::Matrix3d to convert. - /// \return The equivalent Eigen::Matrix3d. - inline Eigen::Matrix3d convert(const ignition::math::Matrix3d &_m) - { - Eigen::Matrix3d matrix; - for (std::size_t i=0; i < 3; ++i) - { - for (std::size_t j=0; j < 3; ++j) - { - matrix(i, j) = _m(i, j); - } - } - - 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. - inline Eigen::Quaterniond convert(const ignition::math::Quaterniond &_q) - { - Eigen::Quaterniond quat; - quat.w() = _q.W(); - quat.x() = _q.X(); - quat.y() = _q.Y(); - quat.z() = _q.Z(); - - return quat; - } - - /// \brief Convert ignition::math::Pose3d to Eigen::Isometry3d. - /// \param[in] _pose ignition::math::Pose3d to convert. - /// \return The equivalent Eigen::Isometry3d. - inline Eigen::Isometry3d convert(const ignition::math::Pose3d &_pose) - { - Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); - tf.translation() = convert(_pose.Pos()); - tf.linear() = Eigen::Matrix3d(convert(_pose.Rot())); - - return tf; - } - - /// \brief Convert Eigen::Vector3d to ignition::math::Vector3d. - /// \param[in] _v Eigen::Vector3d to convert. - /// \return The equivalent ignition::math::Vector3d. - inline ignition::math::Vector3d convert(const Eigen::Vector3d &_v) - { - ignition::math::Vector3d vec; - vec.X() = _v[0]; - vec.Y() = _v[1]; - vec.Z() = _v[2]; - - return vec; - } - - /// \brief Convert Eigen::AlignedBox3d to ignition::math::AxisAlignedBox. - /// \param[in] _b Eigen::AlignedBox3d to convert. - /// \return The equivalent ignition::math::AxisAlignedBox. - inline ignition::math::AxisAlignedBox convert( - const Eigen::AlignedBox3d &_b) - { - ignition::math::AxisAlignedBox box; - box.Min() = convert(_b.min()); - box.Max() = convert(_b.max()); - - return box; - } - - /// \brief Convert Eigen::Matrix3d to ignition::math::Matrix3d. - /// \param[in] _m Eigen::Matrix3d to convert. - /// \return The equivalent ignition::math::Matrix3d. - inline ignition::math::Matrix3d convert(const Eigen::Matrix3d &_m) - { - ignition::math::Matrix3d matrix; - for (std::size_t i=0; i < 3; ++i) - { - for (std::size_t j=0; j < 3; ++j) - { - matrix(i, j) = _m(i, j); - } - } - - 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. - inline ignition::math::Quaterniond convert(const Eigen::Quaterniond &_q) - { - ignition::math::Quaterniond quat; - quat.W() = _q.w(); - quat.X() = _q.x(); - quat.Y() = _q.y(); - quat.Z() = _q.z(); - - return quat; - } - - /// \brief Convert Eigen::Isometry3d to ignition::math::Pose3d. - /// \param[in] _tf Eigen::Isometry3d to convert. - /// \return The equivalent ignition::math::Pose3d. - inline ignition::math::Pose3d convert(const Eigen::Isometry3d &_tf) - { - ignition::math::Pose3d pose; - pose.Pos() = convert(Eigen::Vector3d(_tf.translation())); - pose.Rot() = convert(Eigen::Quaterniond(_tf.linear())); - - return pose; - } - } - } -} - -#endif +#include +#include diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 25f740bd8..962463fa3 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,149 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_MATH_EIGEN3_UTIL_HH_ -#define IGNITION_MATH_EIGEN3_UTIL_HH_ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace ignition -{ - namespace math - { - namespace eigen3 - { - /// \brief Get covariance matrix from a set of 3d vertices - /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305 - /// \param[in] _vertices a vector of 3d vertices - /// \return Covariance matrix - inline Eigen::Matrix3d covarianceMatrix( - const std::vector &_vertices) - { - if (_vertices.empty()) - return Eigen::Matrix3d::Identity(); - - Eigen::Matrix cumulants; - cumulants.setZero(); - for (const auto &vertex : _vertices) - { - const Eigen::Vector3d &point = math::eigen3::convert(vertex); - cumulants(0) += point(0); - cumulants(1) += point(1); - cumulants(2) += point(2); - cumulants(3) += point(0) * point(0); - cumulants(4) += point(0) * point(1); - cumulants(5) += point(0) * point(2); - cumulants(6) += point(1) * point(1); - cumulants(7) += point(1) * point(2); - cumulants(8) += point(2) * point(2); - } - - Eigen::Matrix3d covariance; - - cumulants /= static_cast(_vertices.size()); - - covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0); - covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1); - covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2); - covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1); - covariance(1, 0) = covariance(0, 1); - covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2); - covariance(2, 0) = covariance(0, 2); - covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2); - covariance(2, 1) = covariance(1, 2); - return covariance; - } - - /// \brief Get the oriented 3d bounding box of a set of 3d - /// vertices using PCA - /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html - /// \param[in] _vertices a vector of 3d vertices - /// \return Oriented 3D box - inline ignition::math::OrientedBoxd verticesToOrientedBox( - const std::vector &_vertices) - { - math::OrientedBoxd box; - - // Return an empty box if there are no vertices - if (_vertices.empty()) - return box; - - math::Vector3d mean; - for (const auto &point : _vertices) - mean += point; - mean /= static_cast(_vertices.size()); - - Eigen::Vector3d centroid = math::eigen3::convert(mean); - Eigen::Matrix3d covariance = covarianceMatrix(_vertices); - - // Eigen Vectors - Eigen::SelfAdjointEigenSolver - eigenSolver(covariance, Eigen::ComputeEigenvectors); - Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors(); - - // This line is necessary for proper orientation in some cases. - // The numbers come out the same without it, but the signs are - // different and the box doesn't get correctly oriented in some cases. - eigenVectorsPCA.col(2) = - eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); - - // Transform the original cloud to the origin where the principal - // components correspond to the axes. - Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity()); - projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); - projectionTransform.block<3, 1>(0, 3) = - -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid); - - Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32); - Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32); - - // Get the minimum and maximum points of the transformed cloud. - for (const auto &point : _vertices) - { - Eigen::Vector4d pt(0, 0, 0, 1); - pt.head<3>() = math::eigen3::convert(point); - Eigen::Vector4d tfPoint = projectionTransform * pt; - minPoint = minPoint.cwiseMin(tfPoint.head<3>()); - maxPoint = maxPoint.cwiseMax(tfPoint.head<3>()); - } - - const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint); - - // quaternion is calculated using the eigenvectors (which determines - // how the final box gets rotated), and the transform to put the box - // in correct location is calculated - const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA); - const Eigen::Vector3d bboxTransform = - eigenVectorsPCA * meanDiagonal + centroid; - - math::Vector3d size( - maxPoint.x() - minPoint.x(), - maxPoint.y() - minPoint.y(), - maxPoint.z() - minPoint.z() - ); - math::Pose3d pose; - pose.Rot() = math::eigen3::convert(bboxQuaternion); - pose.Pos() = math::eigen3::convert(bboxTransform); - - box.Size(size); - box.Pose(pose); - return box; - } - } - } -} - -#endif +#include +#include diff --git a/eigen3/src/Conversions_TEST.cc b/eigen3/src/Conversions_TEST.cc index 628311407..391f4f52f 100644 --- a/eigen3/src/Conversions_TEST.cc +++ b/eigen3/src/Conversions_TEST.cc @@ -18,29 +18,29 @@ #include -#include +#include ///////////////////////////////////////////////// /// Check Vector3 conversions TEST(EigenConversions, ConvertVector3) { { - ignition::math::Vector3d iVec, iVec2; - Eigen::Vector3d eVec = ignition::math::eigen3::convert(iVec); + gz::math::Vector3d iVec, iVec2; + Eigen::Vector3d eVec = gz::math::eigen3::convert(iVec); EXPECT_DOUBLE_EQ(0, eVec[0]); EXPECT_DOUBLE_EQ(0, eVec[1]); EXPECT_DOUBLE_EQ(0, eVec[2]); - iVec2 = ignition::math::eigen3::convert(eVec); + iVec2 = gz::math::eigen3::convert(eVec); EXPECT_EQ(iVec, iVec2); } { - ignition::math::Vector3d iVec(100.5, -2.314, 42), iVec2; - Eigen::Vector3d eVec = ignition::math::eigen3::convert(iVec); + gz::math::Vector3d iVec(100.5, -2.314, 42), iVec2; + Eigen::Vector3d eVec = gz::math::eigen3::convert(iVec); EXPECT_DOUBLE_EQ(iVec[0], eVec[0]); EXPECT_DOUBLE_EQ(iVec[1], eVec[1]); EXPECT_DOUBLE_EQ(iVec[2], eVec[2]); - iVec2 = ignition::math::eigen3::convert(eVec); + iVec2 = gz::math::eigen3::convert(eVec); EXPECT_EQ(iVec, iVec2); } } @@ -50,31 +50,31 @@ TEST(EigenConversions, ConvertVector3) TEST(EigenConversions, ConvertAxisAlignedBox) { { - ignition::math::AxisAlignedBox iBox, iBox2; - Eigen::AlignedBox3d eBox = ignition::math::eigen3::convert(iBox); - EXPECT_DOUBLE_EQ(ignition::math::MAX_D, eBox.min()[0]); - EXPECT_DOUBLE_EQ(ignition::math::MAX_D, eBox.min()[1]); - EXPECT_DOUBLE_EQ(ignition::math::MAX_D, eBox.min()[2]); - EXPECT_DOUBLE_EQ(ignition::math::LOW_D, eBox.max()[0]); - EXPECT_DOUBLE_EQ(ignition::math::LOW_D, eBox.max()[1]); - EXPECT_DOUBLE_EQ(ignition::math::LOW_D, eBox.max()[2]); - iBox2 = ignition::math::eigen3::convert(eBox); + gz::math::AxisAlignedBox iBox, iBox2; + Eigen::AlignedBox3d eBox = gz::math::eigen3::convert(iBox); + EXPECT_DOUBLE_EQ(gz::math::MAX_D, eBox.min()[0]); + EXPECT_DOUBLE_EQ(gz::math::MAX_D, eBox.min()[1]); + EXPECT_DOUBLE_EQ(gz::math::MAX_D, eBox.min()[2]); + EXPECT_DOUBLE_EQ(gz::math::LOW_D, eBox.max()[0]); + EXPECT_DOUBLE_EQ(gz::math::LOW_D, eBox.max()[1]); + EXPECT_DOUBLE_EQ(gz::math::LOW_D, eBox.max()[2]); + iBox2 = gz::math::eigen3::convert(eBox); EXPECT_EQ(iBox, iBox2); } { - ignition::math::AxisAlignedBox iBox( - ignition::math::Vector3d(100.5, -2.314, 42), - ignition::math::Vector3d(305, 2.314, 142)); - ignition::math::AxisAlignedBox iBox2; - Eigen::AlignedBox3d eBox = ignition::math::eigen3::convert(iBox); + gz::math::AxisAlignedBox iBox( + gz::math::Vector3d(100.5, -2.314, 42), + gz::math::Vector3d(305, 2.314, 142)); + gz::math::AxisAlignedBox iBox2; + Eigen::AlignedBox3d eBox = gz::math::eigen3::convert(iBox); EXPECT_DOUBLE_EQ(iBox.Min()[0], eBox.min()[0]); EXPECT_DOUBLE_EQ(iBox.Min()[1], eBox.min()[1]); EXPECT_DOUBLE_EQ(iBox.Min()[2], eBox.min()[2]); EXPECT_DOUBLE_EQ(iBox.Max()[0], eBox.max()[0]); EXPECT_DOUBLE_EQ(iBox.Max()[1], eBox.max()[1]); EXPECT_DOUBLE_EQ(iBox.Max()[2], eBox.max()[2]); - iBox2 = ignition::math::eigen3::convert(eBox); + iBox2 = gz::math::eigen3::convert(eBox); EXPECT_EQ(iBox, iBox2); } } @@ -84,24 +84,24 @@ TEST(EigenConversions, ConvertAxisAlignedBox) TEST(EigenConversions, ConvertQuaternion) { { - ignition::math::Quaterniond iQuat, iQuat2; - Eigen::Quaterniond eQuat = ignition::math::eigen3::convert(iQuat); + gz::math::Quaterniond iQuat, iQuat2; + Eigen::Quaterniond eQuat = gz::math::eigen3::convert(iQuat); EXPECT_DOUBLE_EQ(1, eQuat.w()); EXPECT_DOUBLE_EQ(0, eQuat.x()); EXPECT_DOUBLE_EQ(0, eQuat.y()); EXPECT_DOUBLE_EQ(0, eQuat.z()); - iQuat2 = ignition::math::eigen3::convert(eQuat); + iQuat2 = gz::math::eigen3::convert(eQuat); EXPECT_EQ(iQuat, iQuat2); } { - ignition::math::Quaterniond iQuat(0.1, 0.2, 0.3), iQuat2; - Eigen::Quaterniond eQuat = ignition::math::eigen3::convert(iQuat); + gz::math::Quaterniond iQuat(0.1, 0.2, 0.3), iQuat2; + Eigen::Quaterniond eQuat = gz::math::eigen3::convert(iQuat); EXPECT_DOUBLE_EQ(iQuat.W(), eQuat.w()); EXPECT_DOUBLE_EQ(iQuat.X(), eQuat.x()); EXPECT_DOUBLE_EQ(iQuat.Y(), eQuat.y()); EXPECT_DOUBLE_EQ(iQuat.Z(), eQuat.z()); - iQuat2 = ignition::math::eigen3::convert(eQuat); + iQuat2 = gz::math::eigen3::convert(eQuat); EXPECT_EQ(iQuat, iQuat2); } } @@ -111,8 +111,8 @@ TEST(EigenConversions, ConvertQuaternion) TEST(EigenConversions, ConvertMatrix3) { { - ignition::math::Matrix3d iMat, iMat2; - Eigen::Matrix3d eMat = ignition::math::eigen3::convert(iMat); + gz::math::Matrix3d iMat, iMat2; + Eigen::Matrix3d eMat = gz::math::eigen3::convert(iMat); EXPECT_DOUBLE_EQ(0, eMat(0, 0)); EXPECT_DOUBLE_EQ(0, eMat(0, 1)); EXPECT_DOUBLE_EQ(0, eMat(0, 2)); @@ -122,14 +122,14 @@ TEST(EigenConversions, ConvertMatrix3) EXPECT_DOUBLE_EQ(0, eMat(2, 0)); EXPECT_DOUBLE_EQ(0, eMat(2, 1)); EXPECT_DOUBLE_EQ(0, eMat(2, 2)); - iMat2 = ignition::math::eigen3::convert(eMat); + iMat2 = gz::math::eigen3::convert(eMat); EXPECT_EQ(iMat, iMat2); } { - ignition::math::Matrix3d iMat(1, 2, 3, 4, 5, 6, 7, 8, 9), iMat2; - Eigen::Matrix3d eMat = ignition::math::eigen3::convert(iMat); + gz::math::Matrix3d iMat(1, 2, 3, 4, 5, 6, 7, 8, 9), iMat2; + Eigen::Matrix3d eMat = gz::math::eigen3::convert(iMat); EXPECT_DOUBLE_EQ(iMat(0, 0), eMat(0, 0)); EXPECT_DOUBLE_EQ(iMat(0, 1), eMat(0, 1)); EXPECT_DOUBLE_EQ(iMat(0, 2), eMat(0, 2)); @@ -139,7 +139,7 @@ TEST(EigenConversions, ConvertMatrix3) EXPECT_DOUBLE_EQ(iMat(2, 0), eMat(2, 0)); EXPECT_DOUBLE_EQ(iMat(2, 1), eMat(2, 1)); EXPECT_DOUBLE_EQ(iMat(2, 2), eMat(2, 2)); - iMat2 = ignition::math::eigen3::convert(eMat); + iMat2 = gz::math::eigen3::convert(eMat); EXPECT_EQ(iMat, iMat2); } } @@ -149,8 +149,8 @@ TEST(EigenConversions, ConvertMatrix3) TEST(EigenConversions, ConvertMatrix6) { { - ignition::math::Matrix6d iMat, iMat2; - auto eMat = ignition::math::eigen3::convert(iMat); + gz::math::Matrix6d iMat, iMat2; + auto eMat = gz::math::eigen3::convert(iMat); for (std::size_t i = 0; i < 6; ++i) { for (std::size_t j = 0; j < 6; ++j) @@ -158,20 +158,20 @@ TEST(EigenConversions, ConvertMatrix6) EXPECT_DOUBLE_EQ(0.0, eMat(i, j)); } } - iMat2 = ignition::math::eigen3::convert(eMat); + iMat2 = gz::math::eigen3::convert(eMat); EXPECT_EQ(iMat, iMat2); } { - ignition::math::Matrix6d iMat( + gz::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); + gz::math::Matrix6d iMat2; + auto eMat = gz::math::eigen3::convert(iMat); for (std::size_t i = 0; i < 6; ++i) { for (std::size_t j = 0; j < 6; ++j) @@ -179,7 +179,7 @@ TEST(EigenConversions, ConvertMatrix6) EXPECT_DOUBLE_EQ(iMat(i, j), eMat(i, j)); } } - iMat2 = ignition::math::eigen3::convert(eMat); + iMat2 = gz::math::eigen3::convert(eMat); EXPECT_EQ(iMat, iMat2); } } @@ -189,8 +189,8 @@ TEST(EigenConversions, ConvertMatrix6) TEST(EigenConversions, ConvertPose3) { { - ignition::math::Pose3d iPose, iPose2; - Eigen::Isometry3d ePose = ignition::math::eigen3::convert(iPose); + gz::math::Pose3d iPose, iPose2; + Eigen::Isometry3d ePose = gz::math::eigen3::convert(iPose); Eigen::Vector3d eVec = ePose.translation(); EXPECT_DOUBLE_EQ(0, eVec[0]); EXPECT_DOUBLE_EQ(0, eVec[1]); @@ -200,14 +200,14 @@ TEST(EigenConversions, ConvertPose3) EXPECT_DOUBLE_EQ(0, eQuat.x()); EXPECT_DOUBLE_EQ(0, eQuat.y()); EXPECT_DOUBLE_EQ(0, eQuat.z()); - iPose2 = ignition::math::eigen3::convert(ePose); + iPose2 = gz::math::eigen3::convert(ePose); EXPECT_EQ(iPose, iPose2); } { - ignition::math::Pose3d iPose(105.4, 3.1, -0.34, 3.14/8, 3.14/16, -3.14/2); - ignition::math::Pose3d iPose2; - Eigen::Isometry3d ePose = ignition::math::eigen3::convert(iPose); + gz::math::Pose3d iPose(105.4, 3.1, -0.34, 3.14/8, 3.14/16, -3.14/2); + gz::math::Pose3d iPose2; + Eigen::Isometry3d ePose = gz::math::eigen3::convert(iPose); Eigen::Vector3d eVec = ePose.translation(); EXPECT_DOUBLE_EQ(iPose.Pos()[0], eVec[0]); EXPECT_DOUBLE_EQ(iPose.Pos()[1], eVec[1]); @@ -217,7 +217,7 @@ TEST(EigenConversions, ConvertPose3) EXPECT_DOUBLE_EQ(iPose.Rot().X(), eQuat.x()); EXPECT_DOUBLE_EQ(iPose.Rot().Y(), eQuat.y()); EXPECT_DOUBLE_EQ(iPose.Rot().Z(), eQuat.z()); - iPose2 = ignition::math::eigen3::convert(ePose); + iPose2 = gz::math::eigen3::convert(ePose); EXPECT_EQ(iPose, iPose2); } } diff --git a/eigen3/src/Util_TEST.cc b/eigen3/src/Util_TEST.cc index 1e5220462..a4281ecc3 100644 --- a/eigen3/src/Util_TEST.cc +++ b/eigen3/src/Util_TEST.cc @@ -17,9 +17,9 @@ #include -#include +#include -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// /// \brief Test the oriented box converted from a set of vertices diff --git a/examples/additively_separable_scalar_field3_example.cc b/examples/additively_separable_scalar_field3_example.cc index c46ffd5ee..20670c59a 100644 --- a/examples/additively_separable_scalar_field3_example.cc +++ b/examples/additively_separable_scalar_field3_example.cc @@ -16,21 +16,21 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { const double kConstant = 1.; - const ignition::math::Polynomial3d xPoly( - ignition::math::Vector4d(0., 1., 0., 1.)); - const ignition::math::Polynomial3d yPoly( - ignition::math::Vector4d(1., 0., 1., 0.)); - const ignition::math::Polynomial3d zPoly( - ignition::math::Vector4d(1., 0., 0., -1.)); + const gz::math::Polynomial3d xPoly( + gz::math::Vector4d(0., 1., 0., 1.)); + const gz::math::Polynomial3d yPoly( + gz::math::Vector4d(1., 0., 1., 0.)); + const gz::math::Polynomial3d zPoly( + gz::math::Vector4d(1., 0., 0., -1.)); using AdditivelySeparableScalarField3dT = - ignition::math::AdditivelySeparableScalarField3d< - ignition::math::Polynomial3d>; + gz::math::AdditivelySeparableScalarField3d< + gz::math::Polynomial3d>; const AdditivelySeparableScalarField3dT scalarField( kConstant, xPoly, yPoly, zPoly); @@ -41,7 +41,7 @@ int main(int argc, char **argv) // An additively separable scalar field can be evaluated. std::cout << "Evaluating F(x, y, z) at (0, 1, 0) yields " - << scalarField(ignition::math::Vector3d::UnitY) + << scalarField(gz::math::Vector3d::UnitY) << std::endl; // An additively separable scalar field can be queried for its diff --git a/examples/angle_example.cc b/examples/angle_example.cc index eb2ea916c..e0390e529 100644 --- a/examples/angle_example.cc +++ b/examples/angle_example.cc @@ -16,20 +16,20 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { //! [Create an angle] - ignition::math::Angle a; + gz::math::Angle a; //! [Create an angle] // A default constructed angle should be zero. std::cout << "The angle 'a' should be zero: " << a << std::endl; //! [constant pi] - a = ignition::math::Angle::HalfPi; - a = ignition::math::Angle::Pi; + a = gz::math::Angle::HalfPi; + a = gz::math::Angle::Pi; //! [constant pi] //! [Output the angle in radians and degrees.] @@ -38,7 +38,7 @@ int main(int argc, char **argv) //! [Output the angle in radians and degrees.] //! [The Angle class overloads the +=, and many other, math operators.] - a += ignition::math::Angle::HalfPi; + a += gz::math::Angle::HalfPi; //! [The Angle class overloads the +=, and many other, math operators.] std::cout << "Pi + PI/2 in radians: " << a << std::endl; //! [normalized] diff --git a/examples/color_example.cc b/examples/color_example.cc index 4d7325a9e..8d0419234 100644 --- a/examples/color_example.cc +++ b/examples/color_example.cc @@ -16,31 +16,31 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { //! [Create a color] - ignition::math::Color a = ignition::math::Color(0.3, 0.4, 0.5); + gz::math::Color a = gz::math::Color(0.3, 0.4, 0.5); //! [Create a color] // The channel order is R, G, B, A and the default alpha value of a should be 1.0 std::cout << "The alpha value of a should be 1: " << a.A() << std::endl; //! [Set a new color value] - a.ignition::math::Color::Set(0.6, 0.7, 0.8, 1.0); + a.gz::math::Color::Set(0.6, 0.7, 0.8, 1.0); //! [Set a new color value] std::cout << "The RGBA value of a: " << a << std::endl; //! [Set value from BGRA] // 0xFF0000FF is blue in BGRA. Convert it to RGBA. - ignition::math::Color::BGRA blue = 0xFF0000FF; - a.ignition::math::Color::SetFromBGRA(blue); + gz::math::Color::BGRA blue = 0xFF0000FF; + a.gz::math::Color::SetFromBGRA(blue); //! [Set value from BGRA] //! [Math operator] - std::cout << "Check if a is Blue: " << (a == ignition::math::Color::Blue) << std::endl; + std::cout << "Check if a is Blue: " << (a == gz::math::Color::Blue) << std::endl; std::cout << "The RGB value of a should be (0, 0, 1): " << a[0] << ", " << a[1] << ", " << a[2] << std::endl; @@ -48,7 +48,7 @@ int main(int argc, char **argv) //! [Set value from HSV] // Initialize with HSV. (240, 1.0, 1.0) is blue in HSV. - a.ignition::math::Color::SetFromHSV(240.0, 1.0, 1.0); + a.gz::math::Color::SetFromHSV(240.0, 1.0, 1.0); std::cout << "The HSV value of a: " << a.HSV() << std::endl; std::cout << "The RGBA value of a should be (0, 0, 1, 1): " << a << std::endl; //! [Set value from HSV] diff --git a/examples/diff_drive_odometry.cc b/examples/diff_drive_odometry.cc index 6f2bcdf8e..7b0a2be06 100644 --- a/examples/diff_drive_odometry.cc +++ b/examples/diff_drive_odometry.cc @@ -18,15 +18,15 @@ #include #include -#include -#include -#include +#include +#include +#include int main(int argc, char **argv) { //! [Create a DiffDriveOdometry] - ignition::math::DiffDriveOdometry odom; + gz::math::DiffDriveOdometry odom; //! [Create an DiffDriveOdometry] double wheelSeparation = 2.0; diff --git a/examples/gauss_markov_process_example.cc b/examples/gauss_markov_process_example.cc index 313bd6272..1b0bf03fc 100644 --- a/examples/gauss_markov_process_example.cc +++ b/examples/gauss_markov_process_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include // You can plot the data generated by this program by following these // steps. @@ -33,7 +33,7 @@ int main(int argc, char **argv) // * Theta (rate at which the process should approach the mean) of 0.1 // * Mu (mean value) 0. // * Sigma (volatility) of 0.5. - ignition::math::GaussMarkovProcess gmp(20.2, 0.1, 0, 0.5); + gz::math::GaussMarkovProcess gmp(20.2, 0.1, 0, 0.5); std::chrono::steady_clock::duration dt = std::chrono::milliseconds(100); diff --git a/examples/graph_example.cc b/examples/graph_example.cc index a30788d48..6552ecc0c 100644 --- a/examples/graph_example.cc +++ b/examples/graph_example.cc @@ -16,13 +16,13 @@ */ #include -#include +#include int main(int argc, char **argv) { // Create a directed graph that is capable of storing integer data in the // vertices and double data on the edges. - ignition::math::graph::DirectedGraph graph( + gz::math::graph::DirectedGraph graph( // Create the vertices, with default data and vertex ids. { {"vertex1"}, {"vertex2"}, {"vertex3"} @@ -40,7 +40,7 @@ int main(int argc, char **argv) << graph << std::endl; // You can assign data to vertices. - ignition::math::graph::DirectedGraph graph2( + gz::math::graph::DirectedGraph graph2( // Create the vertices, with custom data and default vertex ids. { {"vertex1", 1}, {"vertex2", 2}, {"vertex3", 10} @@ -58,7 +58,7 @@ int main(int argc, char **argv) << graph2 << std::endl; // It's also possible to specify vertex ids. - ignition::math::graph::DirectedGraph graph3( + gz::math::graph::DirectedGraph graph3( // Create the vertices with custom data and vertex ids. { {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} @@ -73,7 +73,7 @@ int main(int argc, char **argv) << graph3 << std::endl; // Finally, you can also assign weights to the edges. - ignition::math::graph::DirectedGraph graph4( + gz::math::graph::DirectedGraph graph4( // Create the vertices with custom data and vertex ids. { {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} diff --git a/examples/interval_example.cc b/examples/interval_example.cc index 68623ca72..18f95fb47 100644 --- a/examples/interval_example.cc +++ b/examples/interval_example.cc @@ -16,27 +16,27 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { std::cout << std::boolalpha; - const ignition::math::Intervald defaultInterval; + const gz::math::Intervald defaultInterval; // A default constructed interval should be empty. std::cout << "The " << defaultInterval << " interval is empty: " << defaultInterval.Empty() << std::endl; - const ignition::math::Intervald openInterval = - ignition::math::Intervald::Open(-1., 1.); + const gz::math::Intervald openInterval = + gz::math::Intervald::Open(-1., 1.); // An open interval should exclude its endpoints. std::cout << "The " << openInterval << " interval contains its endpoints: " << (openInterval.Contains(openInterval.LeftValue()) || openInterval.Contains(openInterval.RightValue())) << std::endl; - const ignition::math::Intervald closedInterval = - ignition::math::Intervald::Closed(0., 1.); + const gz::math::Intervald closedInterval = + gz::math::Intervald::Closed(0., 1.); // A closed interval should include its endpoints. std::cout << "The " << closedInterval << " interval contains its endpoints: " @@ -50,10 +50,10 @@ int main(int argc, char **argv) << std::endl; // The unbounded interval should include all non-empty intervals. - std::cout << "The " << ignition::math::Intervald::Unbounded + std::cout << "The " << gz::math::Intervald::Unbounded << " interval contains all previous non-empty intervals: " - << (ignition::math::Intervald::Unbounded.Contains(openInterval) || - ignition::math::Intervald::Unbounded.Contains(closedInterval)) + << (gz::math::Intervald::Unbounded.Contains(openInterval) || + gz::math::Intervald::Unbounded.Contains(closedInterval)) << std::endl; } diff --git a/examples/kmeans.cc b/examples/kmeans.cc index 6f02d2091..868d9f938 100644 --- a/examples/kmeans.cc +++ b/examples/kmeans.cc @@ -18,37 +18,37 @@ #include #include -#include +#include int main(int argc, char **argv) { // Create some observations. - std::vector obs; - obs.push_back(ignition::math::Vector3d(1.0, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(1.1, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(1.2, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(1.3, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(1.4, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(5.0, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(5.1, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(5.2, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(5.3, 1.0, 0.0)); - obs.push_back(ignition::math::Vector3d(5.4, 1.0, 0.0)); + std::vector obs; + obs.push_back(gz::math::Vector3d(1.0, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(1.1, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(1.2, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(1.3, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(1.4, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(5.0, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(5.1, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(5.2, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(5.3, 1.0, 0.0)); + obs.push_back(gz::math::Vector3d(5.4, 1.0, 0.0)); // Initialize Kmeans with two partitions. - ignition::math::Kmeans kmeans(obs); + gz::math::Kmeans kmeans(obs); - std::vector obsCopy; + std::vector obsCopy; obsCopy = kmeans.Observations(); for (auto &elem : obsCopy) - elem += ignition::math::Vector3d(0.1, 0.2, 0.0); + elem += gz::math::Vector3d(0.1, 0.2, 0.0); // append more observations kmeans.AppendObservations(obsCopy); // cluster - std::vector centroids; + std::vector centroids; std::vector labels; auto result = kmeans.Cluster(2, centroids, labels); diff --git a/examples/piecewise_scalar_field3_example.cc b/examples/piecewise_scalar_field3_example.cc index d584ff09c..47bfdf92b 100644 --- a/examples/piecewise_scalar_field3_example.cc +++ b/examples/piecewise_scalar_field3_example.cc @@ -17,38 +17,38 @@ //! [complete] #include -#include -#include -#include +#include +#include +#include int main(int argc, char **argv) { const double kConstant = 1.; - const ignition::math::Polynomial3d xPoly( - ignition::math::Vector4d(0., 1., 0., 1.)); - const ignition::math::Polynomial3d yPoly( - ignition::math::Vector4d(1., 0., 1., 0.)); - const ignition::math::Polynomial3d zPoly( - ignition::math::Vector4d(1., 0., 0., -1.)); + const gz::math::Polynomial3d xPoly( + gz::math::Vector4d(0., 1., 0., 1.)); + const gz::math::Polynomial3d yPoly( + gz::math::Vector4d(1., 0., 1., 0.)); + const gz::math::Polynomial3d zPoly( + gz::math::Vector4d(1., 0., 0., -1.)); using AdditivelySeparableScalarField3dT = - ignition::math::AdditivelySeparableScalarField3d< - ignition::math::Polynomial3d>; + gz::math::AdditivelySeparableScalarField3d< + gz::math::Polynomial3d>; using PiecewiseScalarField3dT = - ignition::math::PiecewiseScalarField3d< + gz::math::PiecewiseScalarField3d< AdditivelySeparableScalarField3dT>; const PiecewiseScalarField3dT scalarField({ - {ignition::math::Region3d( // x < 0 halfspace - ignition::math::Intervald::Open( - -ignition::math::INF_D, 0.), - ignition::math::Intervald::Unbounded, - ignition::math::Intervald::Unbounded), + {gz::math::Region3d( // x < 0 halfspace + gz::math::Intervald::Open( + -gz::math::INF_D, 0.), + gz::math::Intervald::Unbounded, + gz::math::Intervald::Unbounded), AdditivelySeparableScalarField3dT( kConstant, xPoly, yPoly, zPoly)}, - {ignition::math::Region3d( // x >= 0 halfspace - ignition::math::Intervald::LeftClosed( - 0., ignition::math::INF_D), - ignition::math::Intervald::Unbounded, - ignition::math::Intervald::Unbounded), + {gz::math::Region3d( // x >= 0 halfspace + gz::math::Intervald::LeftClosed( + 0., gz::math::INF_D), + gz::math::Intervald::Unbounded, + gz::math::Intervald::Unbounded), AdditivelySeparableScalarField3dT( -kConstant, xPoly, yPoly, zPoly)}}); @@ -59,10 +59,10 @@ int main(int argc, char **argv) // A piecewise scalar field can be evaluated. std::cout << "Evaluating P(x, y, z) at (1, 0, 0) yields " - << scalarField(ignition::math::Vector3d::UnitX) + << scalarField(gz::math::Vector3d::UnitX) << std::endl; std::cout << "Evaluating P(x, y, z) at (-1, 0, 0) yields " - << scalarField(-ignition::math::Vector3d::UnitX) + << scalarField(-gz::math::Vector3d::UnitX) << std::endl; // A piecewise scalar field can be queried for its minimum diff --git a/examples/polynomial3_example.cc b/examples/polynomial3_example.cc index f268dbdec..6862b5876 100644 --- a/examples/polynomial3_example.cc +++ b/examples/polynomial3_example.cc @@ -16,22 +16,22 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { // A default constructed polynomial should have zero coefficients. std::cout << "A default constructed polynomial is always: " - << ignition::math::Polynomial3d() << std::endl; + << gz::math::Polynomial3d() << std::endl; // A constant polynomial only has an independent term. std::cout << "A constant polynomial only has an independent term: " - << ignition::math::Polynomial3d::Constant(-1.) << std::endl; + << gz::math::Polynomial3d::Constant(-1.) << std::endl; // A cubic polynomial may be incomplete. - const ignition::math::Polynomial3d p( - ignition::math::Vector4d(1., 0., -1., 2.)); + const gz::math::Polynomial3d p( + gz::math::Vector4d(1., 0., -1., 2.)); std::cout << "A cubic polynomial may be incomplete: " << p << std::endl; // A polynomial can be evaluated. @@ -41,15 +41,15 @@ int main(int argc, char **argv) // A polynomial can be queried for its minimum in a given interval, // as well as for its global minimum (which may not always be finite). - const ignition::math::Intervald interval = - ignition::math::Intervald::Closed(-1, 1.); + const gz::math::Intervald interval = + gz::math::Intervald::Closed(-1, 1.); std::cout << "The minimum of " << p << " in the " << interval << " interval is " << p.Minimum(interval) << std::endl; std::cout << "The global minimum of " << p << " is " << p.Minimum() << std::endl; - const ignition::math::Polynomial3d q( - ignition::math::Vector4d(0., 1., 2., 1.)); + const gz::math::Polynomial3d q( + gz::math::Vector4d(0., 1., 2., 1.)); std::cout << "The minimum of " << q << " in the " << interval << " interval is " << q.Minimum(interval) << std::endl; std::cout << "The global minimum of " << q diff --git a/examples/quaternion_from_euler.cc b/examples/quaternion_from_euler.cc index 0f3fb16c8..b1353c0b5 100644 --- a/examples/quaternion_from_euler.cc +++ b/examples/quaternion_from_euler.cc @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include // Copied from urdfdom static inline double strToDouble(const char *in) @@ -70,8 +70,8 @@ int main(int argc, char **argv) IGN_RTOD(yaw)); //![constructor] - ignition::math::Quaterniond q(roll, pitch, yaw); - ignition::math::Matrix3d m(q); + gz::math::Quaterniond q(roll, pitch, yaw); + gz::math::Matrix3d m(q); //![constructor] std::cout << "\nto Quaternion\n"; diff --git a/examples/quaternion_to_euler.cc b/examples/quaternion_to_euler.cc index 0fe1d0566..858b32be8 100644 --- a/examples/quaternion_to_euler.cc +++ b/examples/quaternion_to_euler.cc @@ -17,9 +17,9 @@ #include #include -#include -#include -#include +#include +#include +#include // Copied from urdfdom static inline double strToDouble(const char *in) @@ -65,7 +65,7 @@ int main(int argc, char **argv) << "\n Y " << y << "\n Z " << z << std::endl; - ignition::math::Quaterniond q(w, x, y, z); + gz::math::Quaterniond q(w, x, y, z); q.Normalize(); std::cout << "to" << "\n W " << q.W() @@ -74,9 +74,9 @@ int main(int argc, char **argv) << "\n Z " << q.Z() << std::endl; - ignition::math::Matrix3d m(q); + gz::math::Matrix3d m(q); //![constructor] - ignition::math::Vector3d euler(q.Euler()); + gz::math::Vector3d euler(q.Euler()); //![constructor] std::cout << "\nConverting to Euler angles\n"; diff --git a/examples/rand_example.cc b/examples/rand_example.cc index 227e4399a..08dbd9c11 100644 --- a/examples/rand_example.cc +++ b/examples/rand_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include // You can plot the data generated by this program by following these // steps. @@ -40,11 +40,11 @@ int main(int argc, char **argv) double value; if (std::string(argv[1]) == "normal") { - value = ignition::math::Rand::DblNormal(0, 100); + value = gz::math::Rand::DblNormal(0, 100); } else if (std::string(argv[1]) == "uniform") { - value = ignition::math::Rand::DblUniform(0, 1000); + value = gz::math::Rand::DblUniform(0, 1000); } std::cout << value << std::endl; } diff --git a/examples/region3_example.cc b/examples/region3_example.cc index 1cb647c01..97af405c7 100644 --- a/examples/region3_example.cc +++ b/examples/region3_example.cc @@ -16,33 +16,33 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { std::cout << std::boolalpha; - const ignition::math::Region3d defaultRegion; + const gz::math::Region3d defaultRegion; // A default constructed region should be empty. std::cout << "The " << defaultRegion << " region is empty: " << defaultRegion.Empty() << std::endl; - const ignition::math::Region3d openRegion = - ignition::math::Region3d::Open(-1., -1., -1., 1., 1., 1.); + const gz::math::Region3d openRegion = + gz::math::Region3d::Open(-1., -1., -1., 1., 1., 1.); // An open region should exclude points on its boundary. std::cout << "The " << openRegion << " region contains the " - << ignition::math::Vector3d::UnitX << " point: " - << openRegion.Contains(ignition::math::Vector3d::UnitX) + << gz::math::Vector3d::UnitX << " point: " + << openRegion.Contains(gz::math::Vector3d::UnitX) << std::endl; - const ignition::math::Region3d closedRegion = - ignition::math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + const gz::math::Region3d closedRegion = + gz::math::Region3d::Closed(0., 0., 0., 1., 1., 1.); // A closed region should include points on its boundary. std::cout << "The " << closedRegion << " region contains the " - << ignition::math::Vector3d::UnitX << " point: " - << closedRegion.Contains(ignition::math::Vector3d::UnitX) + << gz::math::Vector3d::UnitX << " point: " + << closedRegion.Contains(gz::math::Vector3d::UnitX) << std::endl; // Closed and open regions may intersect. @@ -51,10 +51,10 @@ int main(int argc, char **argv) << std::endl; // The unbounded region should include all non-empty regions. - std::cout << "The " << ignition::math::Region3d::Unbounded + std::cout << "The " << gz::math::Region3d::Unbounded << " region contains all previous non-empty intervals: " - << (ignition::math::Region3d::Unbounded.Contains(openRegion) || - ignition::math::Region3d::Unbounded.Contains(closedRegion)) + << (gz::math::Region3d::Unbounded.Contains(openRegion) || + gz::math::Region3d::Unbounded.Contains(closedRegion)) << std::endl; } diff --git a/examples/triangle_example.cc b/examples/triangle_example.cc index a2adeaf99..a6d849e97 100644 --- a/examples/triangle_example.cc +++ b/examples/triangle_example.cc @@ -16,7 +16,7 @@ */ #include -#include +#include int main(int argc, char **argv) { @@ -25,10 +25,10 @@ int main(int argc, char **argv) // 2: x = 0, y = 1 // 3: x = 1, y = 0 //! [constructor] - ignition::math::Triangled tri( - ignition::math::Vector2d(-1, 0), - ignition::math::Vector2d(0, 1), - ignition::math::Vector2d(1, 0)); + gz::math::Triangled tri( + gz::math::Vector2d(-1, 0), + gz::math::Vector2d(0, 1), + gz::math::Vector2d(1, 0)); //! [constructor] // The individual vertices are accessible through the [] operator @@ -48,16 +48,16 @@ int main(int argc, char **argv) // It's also possible to set each vertex individually. //! [vertex1] - tri.Set(0, ignition::math::Vector2d(-10, 0)); - tri.Set(1, ignition::math::Vector2d(0, 20)); - tri.Set(2, ignition::math::Vector2d(10, 2)); + tri.Set(0, gz::math::Vector2d(-10, 0)); + tri.Set(1, gz::math::Vector2d(0, 20)); + tri.Set(2, gz::math::Vector2d(10, 2)); //! [vertex1] // Or set all the vertices at once. //! [vertex2] - tri.Set(ignition::math::Vector2d(-1, 0), - ignition::math::Vector2d(0, 1), - ignition::math::Vector2d(1, 0)); + tri.Set(gz::math::Vector2d(-1, 0), + gz::math::Vector2d(0, 1), + gz::math::Vector2d(1, 0)); //! [vertex2] // You can get the perimeter length and area of the triangle @@ -68,7 +68,7 @@ int main(int argc, char **argv) // The Contains functions check if a line or point is inside the triangle //! [contains] - if (tri.Contains(ignition::math::Vector2d(0, 0.5))) + if (tri.Contains(gz::math::Vector2d(0, 0.5))) std::cout << "Triangle contains the point 0, 0.5\n"; else std::cout << "Triangle does not contain the point 0, 0.5\n"; @@ -77,8 +77,8 @@ int main(int argc, char **argv) // The Intersect function check if a line segment intersects the triangle. // It also returns the points of intersection //! [intersect] - ignition::math::Vector2d pt1, pt2; - if (tri.Intersects(ignition::math::Line2d(-2, 0.5, 2, 0.5), pt1, pt2)) + gz::math::Vector2d pt1, pt2; + if (tri.Intersects(gz::math::Line2d(-2, 0.5, 2, 0.5), pt1, pt2)) { std::cout << "A line from (-2, 0.5) to (2, 0.5) intersects " << "the triangle at the\nfollowing points:\n" diff --git a/examples/vector2_example.cc b/examples/vector2_example.cc index e558a2327..6c63a81d7 100644 --- a/examples/vector2_example.cc +++ b/examples/vector2_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { @@ -24,14 +24,14 @@ int main(int argc, char **argv) // The initial x any y values are zero.\n\n"; // The x and y component of vec2 can be set at anytime. //! [constructor] - ignition::math::Vector2d vec2; + gz::math::Vector2d vec2; vec2.Set(2.0, 4.0); //! [constructor] // The Vector2 class is a template, so you can also create a Vector2 using - // ignition::math::Vector2 + // gz::math::Vector2 //! [constructor2] - ignition::math::Vector2 vec2a; + gz::math::Vector2 vec2a; vec2a.Set(1.0, 2.0); //! [constructor2] @@ -39,7 +39,7 @@ int main(int argc, char **argv) // It's also possible to set initial values. This time we are using // a Vector2 of floats //! [constructor3] - ignition::math::Vector2f vec2b(1.2f, 3.4f); + gz::math::Vector2f vec2b(1.2f, 3.4f); //! [constructor3] // We can output the contents of each vector using std::cout diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 25ec89762..4b2bdd7bb 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1 +1,2 @@ -add_subdirectory(ignition) +add_subdirectory(gz) +install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}) diff --git a/include/gz/CMakeLists.txt b/include/gz/CMakeLists.txt new file mode 100644 index 000000000..2767b4e7a --- /dev/null +++ b/include/gz/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(math) diff --git a/include/gz/math/AdditivelySeparableScalarField3.hh b/include/gz/math/AdditivelySeparableScalarField3.hh new file mode 100644 index 000000000..446294482 --- /dev/null +++ b/include/gz/math/AdditivelySeparableScalarField3.hh @@ -0,0 +1,197 @@ +/* + * 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_SEPARABLE_SCALAR_FIELD3_HH_ +#define GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_ + +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /** \class AdditivelySeparableScalarField3\ + * AdditivelySeparableScalarField3.hh\ + * ignition/math/AdditivelySeparableScalarField3.hh + */ + /// \brief The AdditivelySeparableScalarField3 class constructs + /// a scalar field F in R^3 as a sum of scalar functions i.e. + /// F(x, y, z) = k (p(x) + q(y) + r(z)). + /// + /// \tparam ScalarFunctionT a callable type that taking a single ScalarT + /// value as argument and returning another ScalarT value. Additionally: + /// - for AdditivelySeparableScalarField3T to have a stream operator + /// overload, ScalarFunctionT must implement a + /// void Print(std::ostream &, const std::string &) method that streams + /// a representation of it using the given string as argument variable + /// name; + /// - for AdditivelySeparableScalarField3T::Minimum to be callable, + /// ScalarFunctionT must implement a + /// ScalarT Minimum(const Interval &, ScalarT &) method that + /// computes its minimum in the given interval and returns an argument + /// value that yields said minimum. + /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits + /// have been specialized. + /// + /// ## Example + /// + /// \snippet examples/additively_separable_scalar_field3_example.cc complete + template + class AdditivelySeparableScalarField3 + { + /// \brief Constructor + /// \param[in] _k scalar constant + /// \param[in] _p scalar function of x + /// \param[in] _q scalar function of y + /// \param[in] _r scalar function of z + public: AdditivelySeparableScalarField3( + ScalarT _k, ScalarFunctionT _p, + ScalarFunctionT _q, ScalarFunctionT _r) + : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r)) + { + } + + /// \brief Evaluate the scalar field at `_point` + /// \param[in] _point scalar field argument + /// \return the result of evaluating `F(_point)` + public: ScalarT Evaluate(const Vector3 &_point) const + { + return this->k * ( + this->p(_point.X()) + + this->q(_point.Y()) + + this->r(_point.Z())); + } + + /// \brief Call operator overload + /// \see SeparableScalarField3::Evaluate() + /// \param[in] _point scalar field argument + /// \return the result of evaluating `F(_point)` + public: ScalarT operator()(const Vector3 &_point) const + { + return this->Evaluate(_point); + } + + /// \brief Compute scalar field minimum in a `_region` + /// \param[in] _region scalar field argument set to check + /// \param[out] _pMin scalar field argument that yields + /// the minimum, or NaN if `_region` is empty + /// \return the scalar field minimum in the given `_region`, + /// or NaN if `_region` is empty + public: ScalarT Minimum(const Region3 &_region, + Vector3 &_pMin) const + { + if (_region.Empty()) + { + _pMin = Vector3::NaN; + return std::numeric_limits::quiet_NaN(); + } + return this->k * ( + this->p.Minimum(_region.Ix(), _pMin.X()) + + this->q.Minimum(_region.Iy(), _pMin.Y()) + + this->r.Minimum(_region.Iz(), _pMin.Z())); + } + + /// \brief Compute scalar field minimum in a `_region` + /// \param[in] _region scalar field argument set to check + /// \return the scalar field minimum in the given `_region`, + /// or NaN if `_region` is empty + public: ScalarT Minimum(const Region3 &_region) const + { + Vector3 pMin; + return this->Minimum(_region, pMin); + } + + /// \brief Compute scalar field minimum + /// \param[out] _pMin scalar field argument that yields + /// the minimum, or NaN if `_region` is empty + /// \return the scalar field minimum + public: ScalarT Minimum(Vector3 &_pMin) const + { + return this->Minimum(Region3::Unbounded, _pMin); + } + + /// \brief Compute scalar field minimum + /// \return the scalar field minimum + public: ScalarT Minimum() const + { + Vector3 pMin; + return this->Minimum(Region3::Unbounded, pMin); + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _field SeparableScalarField3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, + const gz::math::AdditivelySeparableScalarField3< + ScalarFunctionT, ScalarT> &_field) + { + using std::abs; // enable ADL + constexpr ScalarT epsilon = + std::numeric_limits::epsilon(); + if ((abs(_field.k) - ScalarT(1)) < epsilon) + { + if (_field.k < ScalarT(0)) + { + _out << "-"; + } + } + else + { + _out << _field.k << " "; + } + _out << "[("; + _field.p.Print(_out, "x"); + _out << ") + ("; + _field.q.Print(_out, "y"); + _out << ") + ("; + _field.r.Print(_out, "z"); + return _out << ")]"; + } + + /// \brief Scalar constant + private: ScalarT k; + + /// \brief Scalar function of x + private: ScalarFunctionT p; + + /// \brief Scalar function of y + private: ScalarFunctionT q; + + /// \brief Scalar function of z + private: ScalarFunctionT r; + }; + + template + using AdditivelySeparableScalarField3f = + AdditivelySeparableScalarField3; + + template + using AdditivelySeparableScalarField3d = + AdditivelySeparableScalarField3; + } + } +} +#endif diff --git a/include/gz/math/Angle.hh b/include/gz/math/Angle.hh new file mode 100644 index 000000000..562b3c05e --- /dev/null +++ b/include/gz/math/Angle.hh @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2012 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_ANGLE_HH_ +#define GZ_MATH_ANGLE_HH_ + +#include +#include +#include + +/// \def IGN_RTOD(d) +/// \brief Macro that converts radians to degrees +/// \param[in] r radians +/// \return degrees +#define IGN_RTOD(r) ((r) * 180 / IGN_PI) + +/// \def IGN_DTOR(d) +/// \brief Converts degrees to radians +/// \param[in] d degrees +/// \return radians +#define IGN_DTOR(d) ((d) * IGN_PI / 180) + +/// \def IGN_NORMALIZE(a) +/// \brief Macro that normalizes an angle in the range -Pi to Pi +/// \param[in] a angle +/// \return the angle, in range +#define IGN_NORMALIZE(a) (atan2(sin(a), cos(a))) + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Angle Angle.hh ignition/math/Angle.hh + /// \brief The Angle class is used to simplify and clarify the use of + /// radians and degrees measurements. A default constructed Angle instance + /// has a value of zero radians/degrees. + /// + /// Unless otherwise specified, the Angle class assumes units are in + /// radians. An example of this are the stream insertion (<<) and + /// extraction (>>) operators. + /// + /// ## Example + /// + /// \snippet examples/angle_example.cc complete + class IGNITION_MATH_VISIBLE Angle + { + /// \brief An angle with a value of zero. + /// Equivalent to math::Angle(0). + public: static const Angle Zero; + + /// \brief An angle with a value of Pi. + /// Equivalent to math::Angle(IGN_PI). + public: static const Angle Pi; + + /// \brief An angle with a value of Pi * 0.5. + /// Equivalent to math::Angle(IGN_PI * 0.5). + public: static const Angle HalfPi; + + /// \brief An angle with a value of Pi * 2. + /// Equivalent to math::Angle(IGN_PI * 2). + public: static const Angle TwoPi; + + /// \brief Default constructor that initializes an Angle to zero + /// radians/degrees. + public: Angle(); + + /// \brief Conversion constructor that initializes an Angle to the + /// specified radians. This constructor supports implicit conversion + /// of a double to an Angle. For example: + /// + /// \code + /// Angle a = 3.14; + /// \endcode + // + /// \param[in] _radian The radians used to initialize this Angle. + // cppcheck-suppress noExplicitConstructor + public: Angle(const double _radian); + + /// \brief Copy constructor that initializes this Angle to the value + /// contained in the _angle parameter. + /// \param[in] _angle Angle to copy + public: Angle(const Angle &_angle); + + /// \brief Move constructor + /// \param[in] _angle Angle to move + public: Angle(Angle &&_angle) noexcept; + + /// \brief Destructor + public: virtual ~Angle(); + + /// \brief Copy assignment operator + /// \param[in] _angle Angle to copy + public: Angle& operator=(const Angle &_angle); + + /// \brief Move assignment operator + /// \param[in] _angle Angle to move + public: Angle& operator=(Angle &&_angle) noexcept; + + /// \brief Set the value from an angle in radians. + /// \param[in] _radian Radian value. + /// \sa SetRadian(double) + public: void Radian(double _radian); + + /// \brief Set the value from an angle in radians. + /// \param[in] _radian Radian value. + public: void SetRadian(double _radian); + + /// \brief Set the value from an angle in degrees + /// \param[in] _degree Degree value + /// \sa SetDegree(double) + public: void Degree(double _degree); + + /// \brief Set the value from an angle in degrees + /// \param[in] _degree Degree value + public: void SetDegree(double _degree); + + /// \brief Get the angle in radians. + /// \return Double containing the angle's radian value. + public: double Radian() const; + + /// \brief Get the angle in degrees. + /// \return Double containing the angle's degree value. + public: double Degree() const; + + /// \brief Normalize the angle in the range -Pi to Pi. This + /// modifies the value contained in this Angle instance. + /// \sa Normalized() + public: void Normalize(); + + /// \brief Return the normalized angle in the range -Pi to Pi. This + /// does not modify the value contained in this Angle instance. + /// \return The normalized value of this Angle. + public: Angle Normalized() const; + + /// \brief Return the angle's radian value + /// \return double containing the angle's radian value + public: double operator()() const; + + /// \brief Dereference operator + /// \return Double containing the angle's radian value + public: inline double operator*() const + { + return value; + } + + /// \brief Subtraction operator, result = this - _angle. + /// \param[in] _angle Angle for subtraction. + /// \return The new angle. + public: Angle operator-(const Angle &_angle) const; + + /// \brief Addition operator, result = this + _angle. + /// \param[in] _angle Angle for addition. + /// \return The new angle. + public: Angle operator+(const Angle &_angle) const; + + /// \brief Multiplication operator, result = this * _angle. + /// \param[in] _angle Angle for multiplication. + /// \return The new angle + public: Angle operator*(const Angle &_angle) const; + + /// \brief Division operator, result = this / _angle. + /// \param[in] _angle Angle for division. + /// \return The new angle. + public: Angle operator/(const Angle &_angle) const; + + /// \brief Subtraction set operator, this = this - _angle. + /// \param[in] _angle Angle for subtraction. + /// \return The new angle. + public: Angle operator-=(const Angle &_angle); + + /// \brief Addition set operator, this = this + _angle. + /// \param[in] _angle Angle for addition. + /// \return The new angle. + public: Angle operator+=(const Angle &_angle); + + /// \brief Multiplication set operator, this = this * _angle. + /// \param[in] _angle Angle for multiplication. + /// \return The new angle. + public: Angle operator*=(const Angle &_angle); + + /// \brief Division set operator, this = this / _angle. + /// \param[in] _angle Angle for division. + /// \return The new angle. + public: Angle operator/=(const Angle &_angle); + + /// \brief Equality operator, result = this == _angle. + /// \param[in] _angle Angle to check for equality. + /// \return True if this == _angle. + public: bool operator==(const Angle &_angle) const; + + /// \brief Inequality operator + /// \param[in] _angle Angle to check for inequality. + /// \return True if this != _angle. + public: bool operator!=(const Angle &_angle) const; + + /// \brief Less than operator. + /// \param[in] _angle Angle to check. + /// \return True if this < _angle. + public: bool operator<(const Angle &_angle) const; + + /// \brief Less than or equal operator. + /// \param[in] _angle Angle to check. + /// \return True if this <= _angle. + public: bool operator<=(const Angle &_angle) const; + + /// \brief Greater than operator. + /// \param[in] _angle Angle to check. + /// \return True if this > _angle. + public: bool operator>(const Angle &_angle) const; + + /// \brief Greater than or equal operator. + /// \param[in] _angle Angle to check. + /// \return True if this >= _angle. + public: bool operator>=(const Angle &_angle) const; + + /// \brief Stream insertion operator. Outputs in radians. + /// \param[in] _out Output stream. + /// \param[in] _a Angle to output. + /// \return The output stream. + public: friend std::ostream &operator<<(std::ostream &_out, + const gz::math::Angle &_a) + { + _out << _a.Radian(); + return _out; + } + + /// \brief Stream extraction operator. Assumes input is in radians. + /// \param[in,out] _in Input stream. + /// \param[out] _a Angle to read value into. + /// \return The input stream. + public: friend std::istream &operator>>(std::istream &_in, + gz::math::Angle &_a) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + _in >> _a.value; + return _in; + } + + /// The angle in radians + private: double value{0}; + }; + } + } +} + +#endif diff --git a/include/gz/math/AxisAlignedBox.hh b/include/gz/math/AxisAlignedBox.hh new file mode 100644 index 000000000..8393d6ba1 --- /dev/null +++ b/include/gz/math/AxisAlignedBox.hh @@ -0,0 +1,315 @@ +/* + * Copyright (C) 2012 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_AXISALIGNEDBOX_HH_ +#define GZ_MATH_AXISALIGNEDBOX_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declaration of private data + class AxisAlignedBoxPrivate; + + /// \class AxisAlignedBox AxisAlignedBox.hh ignition/math/AxisAlignedBox.hh + /// \brief Mathematical representation of a box that is aligned along + /// an X,Y,Z axis. + class IGNITION_MATH_VISIBLE AxisAlignedBox + { + /// \brief Default constructor. This constructor will set the box's + /// minimum and maximum corners to the highest (max) and lowest + /// floating point values available to indicate that it is uninitialized. + /// The default box does not intersect any other boxes or contain any + /// points since it has no extent. Its center is the origin and its side + /// lengths are 0. + public: AxisAlignedBox(); + + /// \brief Constructor. This constructor will compute the box's + /// minimum and maximum corners based on the two arguments. + /// \param[in] _vec1 One corner of the box + /// \param[in] _vec2 Another corner of the box + public: AxisAlignedBox(const Vector3d &_vec1, const Vector3d &_vec2); + + /// \brief Constructor. This constructor will compute the box's + /// minimum and maximum corners based on the arguments. + /// \param[in] _vec1X One corner's X position + /// \param[in] _vec1Y One corner's Y position + /// \param[in] _vec1Z One corner's Z position + /// \param[in] _vec2X Other corner's X position + /// \param[in] _vec2Y Other corner's Y position + /// \param[in] _vec2Z Other corner's Z position + public: AxisAlignedBox(double _vec1X, double _vec1Y, double _vec1Z, + double _vec2X, double _vec2Y, double _vec2Z); + + /// \brief Copy Constructor + /// \param[in] _b AxisAlignedBox to copy + public: AxisAlignedBox(const AxisAlignedBox &_b); + + /// \brief Destructor + public: virtual ~AxisAlignedBox(); + + /// \brief Get the length along the x dimension + /// \return Double value of the length in the x dimension + public: double XLength() const; + + /// \brief Get the length along the y dimension + /// \return Double value of the length in the y dimension + public: double YLength() const; + + /// \brief Get the length along the z dimension + /// \return Double value of the length in the z dimension + public: double ZLength() const; + + /// \brief Get the size of the box + /// \return Size of the box + public: math::Vector3d Size() const; + + /// \brief Get the box center + /// \return The center position of the box + public: math::Vector3d Center() const; + + /// \brief Merge a box with this box + /// \param[in] _box AxisAlignedBox to add to this box + public: void Merge(const AxisAlignedBox &_box); + + /// \brief Assignment operator. Set this box to the parameter + /// \param[in] _b AxisAlignedBox to copy + /// \return The new box. + public: AxisAlignedBox &operator=(const AxisAlignedBox &_b); + + /// \brief Addition operator. result = this + _b + /// \param[in] _b AxisAlignedBox to add + /// \return The new box + public: AxisAlignedBox operator+(const AxisAlignedBox &_b) const; + + /// \brief Addition set operator. this = this + _b + /// \param[in] _b AxisAlignedBox to add + /// \return This new box + public: const AxisAlignedBox &operator+=(const AxisAlignedBox &_b); + + /// \brief Equality test operator + /// \param[in] _b AxisAlignedBox to test + /// \return True if equal + public: bool operator==(const AxisAlignedBox &_b) const; + + /// \brief Inequality test operator + /// \param[in] _b AxisAlignedBox to test + /// \return True if not equal + public: bool operator!=(const AxisAlignedBox &_b) const; + + /// \brief Subtract a vector from the min and max values + /// \param _v The vector to use during subtraction + /// \return The new box + public: AxisAlignedBox operator-(const Vector3d &_v); + + /// \brief Add a vector to the min and max values + /// \param _v The vector to use during addition + /// \return The new box + public: AxisAlignedBox operator+(const Vector3d &_v); + + /// \brief Subtract a vector from the min and max values + /// \param _v The vector to use during subtraction + /// \return The new box + public: AxisAlignedBox operator-(const Vector3d &_v) const; + + /// \brief Add a vector to the min and max values + /// \param _v The vector to use during addition + /// \return The new box + public: AxisAlignedBox operator+(const Vector3d &_v) const; + + /// \brief Output operator + /// \param[in] _out Output stream + /// \param[in] _b AxisAlignedBox to output to the stream + /// \return The stream + public: friend std::ostream &operator<<(std::ostream &_out, + const gz::math::AxisAlignedBox &_b) + { + _out << "Min[" << _b.Min() << "] Max[" << _b.Max() << "]"; + return _out; + } + + /// \brief Get the minimum corner. + /// \return The Vector3d that is the minimum corner of the box. + public: const Vector3d &Min() const; + + /// \brief Get the maximum corner. + /// \return The Vector3d that is the maximum corner of the box. + public: const Vector3d &Max() const; + + /// \brief Get a mutable version of the minimum corner. + /// \return The Vector3d that is the minimum corner of the box. + public: Vector3d &Min(); + + /// \brief Get a mutable version of the maximum corner. + /// \return The Vector3d that is the maximum corner of the box. + public: Vector3d &Max(); + + /// \brief Test box intersection. This test will only work if + /// both box's minimum corner is less than or equal to their + /// maximum corner. + /// \param[in] _box AxisAlignedBox to check for intersection with + /// this box. + /// \return True if this box intersects _box. + public: bool Intersects(const AxisAlignedBox &_box) const; + + /// \brief Check if a point lies inside the box. + /// \param[in] _p Point to check. + /// \return True if the point is inside the box. + public: bool Contains(const Vector3d &_p) const; + + /// \brief Check if a ray (origin, direction) intersects the box. + /// \param[in] _origin Origin of the ray. + /// \param[in] _dir Direction of the ray. This ray will be normalized. + /// \param[in] _min Minimum allowed distance. + /// \param[in] _max Maximum allowed distance. + /// \return A boolean + public: bool IntersectCheck(const Vector3d &_origin, const Vector3d &_dir, + const double _min, const double _max) const; + + /// \brief Check if a ray (origin, direction) intersects the box. + /// \param[in] _origin Origin of the ray. + /// \param[in] _dir Direction of the ray. This ray will be normalized. + /// \param[in] _min Minimum allowed distance. + /// \param[in] _max Maximum allowed distance. + /// \return A boolean and double tuple. The boolean value is true + /// if the line intersects the box. + /// + /// The double is the distance from + /// the ray's start to the closest intersection point on the box, + /// minus the _min distance. For example, if _min == 0.5 and the + /// intersection happens at a distance of 2.0 from _origin then returned + /// distance is 1.5. + /// + /// The double value is zero when the boolean value is false. + public: std::tuple IntersectDist( + const Vector3d &_origin, const Vector3d &_dir, + const double _min, const double _max) const; + + /// \brief Check if a ray (origin, direction) intersects the box. + /// \param[in] _origin Origin of the ray. + /// \param[in] _dir Direction of the ray. This ray will be normalized. + /// \param[in] _min Minimum allowed distance. + /// \param[in] _max Maximum allowed distance. + /// \return A boolean, double, Vector3d tuple. The boolean value is true + /// if the line intersects the box. + /// + /// The double is the distance from the ray's start to the closest + /// intersection point on the box, + /// minus the _min distance. For example, if _min == 0.5 and the + /// intersection happens at a distance of 2.0 from _origin then returned + /// distance is 1.5. + /// The double value is zero when the boolean value is false. The + /// + /// Vector3d is the intersection point on the box. The Vector3d value + /// is zero if the boolean value is false. + public: std::tuple Intersect( + const Vector3d &_origin, const Vector3d &_dir, + const double _min, const double _max) const; + + /// \brief Check if a line intersects the box. + /// \param[in] _line The line to check against this box. + /// \return A boolean, double, Vector3d tuple. The boolean value is true + /// if the line intersects the box. The double is the distance from + /// the line's start to the closest intersection point on the box. + /// The double value is zero when the boolean value is false. The + /// Vector3d is the intersection point on the box. The Vector3d value + /// is zero if the boolean value is false. + public: std::tuple Intersect( + const Line3d &_line) const; + + /// \brief Get the volume of the box in m^3. + /// \return Volume of the box in m^3. + public: double Volume() const; + + /// \brief Compute the cylinder's density given a mass value. The + /// cylinder is assumed to be solid with uniform density. This + /// function requires the cylinder's radius and length to be set to + /// values greater than zero. The Material of the cylinder is ignored. + /// \param[in] _mass Mass of the cylinder, in kg. This value should be + /// greater than zero. + /// \return Density of the cylinder in kg/m^3. A negative value is + /// returned if radius, length or _mass is <= 0. + /// \deprecated Unimplemented + public: double IGN_DEPRECATED(6.0) DensityFromMass( + const double _mass) const; + + /// \brief Set the density of this box based on a mass value. + /// Density is computed using + /// double DensityFromMass(const double _mass) const. The + /// box is assumed to be solid with uniform density. This + /// function requires the box's size to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the box, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// box's size or the _mass value are <= 0. + /// \sa double DensityFromMass(const double _mass) const + /// \deprecated Unimplemented + public: bool IGN_DEPRECATED(6.0) SetDensityFromMass( + const double _mass); + + /// \brief Get the material associated with this box. + /// \return The material assigned to this box. + /// \deprecated Unimplemented + public: const gz::math::Material IGN_DEPRECATED(6.0) + &Material() const; + + /// \brief Set the material associated with this box. + /// \param[in] _mat The material assigned to this box + /// \deprecated Unimplemented + public: void IGN_DEPRECATED(6.0) SetMaterial( + const gz::math::Material &_mat); + + /// \brief Get the mass matrix for this box. This function + /// is only meaningful if the box's size and material + /// have been set. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid size (<=0) or density (<=0). + /// \deprecated Unimplemented + public: bool IGN_DEPRECATED(6.0) MassMatrix( + MassMatrix3d &_massMat) const; + + /// \brief Clip a line to a dimension of the box. + /// This is a helper function to Intersects + /// \param[in] _d Dimension of the box(0, 1, or 2). + /// \param[in] _line Line to clip + /// \param[in,out] _low Close distance + /// \param[in,out] _high Far distance + private: bool ClipLine(const int _d, const Line3d &_line, + double &_low, double &_high) const; + + /// \brief Private data pointer + private: AxisAlignedBoxPrivate *dataPtr; + }; + } + } +} +#endif diff --git a/include/gz/math/Box.hh b/include/gz/math/Box.hh new file mode 100644 index 000000000..70ba4aee7 --- /dev/null +++ b/include/gz/math/Box.hh @@ -0,0 +1,225 @@ +/* + * Copyright (C) 2018 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_BOX_HH_ +#define GZ_MATH_BOX_HH_ + +#include +#include +#include +#include +#include + +#include "gz/math/detail/WellOrderedVector.hh" + +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \brief This is the type used for deduplicating and returning the set of + /// intersections. + template + using IntersectionPoints = std::set, WellOrderedVectors>; + + /// \class Box Box.hh ignition/math/Box.hh + /// \brief A representation of a box. All units are in meters. + /// + /// The box class supports defining a size and material properties. + /// See Material for more on material properties. + /// + /// By default, a box's size (length, width, and height) is zero. + /// + /// See AxisAlignedBox for an axis aligned box implementation. + template + class Box + { + /// \brief Default constructor. + public: Box() = default; + + /// \brief Construct a box with specified dimensions. + /// \param[in] _length Length of the box in meters. + /// \param[in] _width Width of the box in meters. + /// \param[in] _height Height of the box in meters. + public: Box(const Precision _length, + const Precision _width, + const Precision _height); + + /// \brief Construct a box with specified dimensions and a material. + /// \param[in] _length Length of the box in meters. + /// \param[in] _width Width of the box in meters. + /// \param[in] _height Height of the box. + /// \param[in] _mat Material property for the box. + public: Box(const Precision _length, const Precision _width, + const Precision _height, + const gz::math::Material &_mat); + + /// \brief Construct a box with specified dimensions, in vector form. + /// \param[in] _size Size of the box. The vector _size has the following + /// mapping: + /// + /// * _size[0] == length in meters + /// * _size[1] == width in meters + /// * _size[2] == height in meters + public: explicit Box(const Vector3 &_size); + + /// \brief Construct a box with specified dimensions, in vector form + /// and a material. + /// \param[in] _size Size of the box. The vector _size has the following + /// mapping: + /// + /// * _size[0] == length in meters + /// * _size[1] == width in meters + /// * _size[2] == height in meters + /// \param[in] _mat Material property for the box. + public: Box(const Vector3 &_size, + const gz::math::Material &_mat); + + /// \brief Destructor. + public: virtual ~Box() = default; + + /// \brief Get the size of the box. + /// \return Size of the box in meters. + public: math::Vector3 Size() const; + + /// \brief Set the size of the box. + /// \param[in] _size Size of the box. The vector _size has the following + /// mapping: + /// + /// * _size[0] == lengt in metersh + /// * _size[1] == widt in metersh + /// * _size[2] == heigh in meterst + public: void SetSize(const math::Vector3 &_size); + + /// \brief Set the size of the box. + /// \param[in] _length Length of the box in meters. + /// \param[in] _width Width of the box in meters. + /// \param[in] _height Height of the box in meters. + public: void SetSize(const Precision _length, + const Precision _width, + const Precision _height); + + /// \brief Equality test operator. + /// \param[in] _b Box to test. + /// \return True if equal. + public: bool operator==(const Box &_b) const; + + /// \brief Inequality test operator. + /// \param[in] _b Box to test. + /// \return True if not equal. + public: bool operator!=(const Box &_b) const; + + /// \brief Get the material associated with this box. + /// \return The material assigned to this box. + public: const gz::math::Material &Material() const; + + /// \brief Set the material associated with this box. + /// \param[in] _mat The material assigned to this box. + public: void SetMaterial(const gz::math::Material &_mat); + + /// \brief Get the volume of the box in m^3. + /// \return Volume of the box in m^3. + public: Precision Volume() const; + + /// \brief Get the volume of the box below a plane. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. + /// \return Volume below the plane in m^3. + public: Precision VolumeBelow(const Plane &_plane) const; + + /// \brief Center of volume below the plane. This is useful when + /// calculating where buoyancy should be applied, for example. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. + /// \return Center of volume, in box's frame. + public: std::optional> + CenterOfVolumeBelow(const Plane &_plane) const; + + /// \brief All the vertices which are on or below the plane. + /// \param[in] _plane The plane which cuts the box, expressed in the box's + /// frame. + /// \return Box vertices which are below the plane, expressed in the box's + /// frame. + public: IntersectionPoints + VerticesBelow(const Plane &_plane) const; + + /// \brief Compute the box's density given a mass value. The + /// box is assumed to be solid with uniform density. This + /// function requires the box's size to be set to + /// values greater than zero. The Material of the box is ignored. + /// \param[in] _mass Mass of the box, in kg. This value should be + /// greater than zero. + /// \return Density of the box in kg/m^3. A negative value is + /// returned if the size or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this box based on a mass value. + /// Density is computed using + /// double DensityFromMass(const double _mass) const. The + /// box is assumed to be solid with uniform density. This + /// function requires the box's size to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the box, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// box's size or the _mass value are <= 0. + /// \sa double DensityFromMass(const double _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Get the mass matrix for this box. This function + /// is only meaningful if the box's size and material + /// have been set. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid size (<=0) or density (<=0). + public: bool MassMatrix(MassMatrix3 &_massMat) const; + + /// \brief Get intersection between a plane and the box's edges. + /// Edges contained on the plane are ignored. + /// \param[in] _plane The plane against which we are testing intersection. + /// \returns A list of points along the edges of the box where the + /// intersection occurs. + public: IntersectionPoints Intersections( + const Plane &_plane) const; + + /// \brief Size of the box. + private: Vector3 size = Vector3::Zero; + + /// \brief The box's material. + private: gz::math::Material material; + }; + + /// \typedef Box Boxi + /// \brief Box with integer precision. + typedef Box Boxi; + + /// \typedef Box Boxd + /// \brief Box with double precision. + typedef Box Boxd; + + /// \typedef Box Boxf + /// \brief Box with float precision. + typedef Box Boxf; + } + } +} +#include "gz/math/detail/Box.hh" +#endif diff --git a/include/ignition/math/CMakeLists.txt b/include/gz/math/CMakeLists.txt similarity index 91% rename from include/ignition/math/CMakeLists.txt rename to include/gz/math/CMakeLists.txt index f1e6bf306..48fadc037 100644 --- a/include/ignition/math/CMakeLists.txt +++ b/include/gz/math/CMakeLists.txt @@ -1,3 +1,3 @@ # Exclude the detail directory from inclusion. The purpose is to prevent the detail/* header files from being included in math.hh. A side effect is that the detail headers are not installed. The next install line solves this problem. ign_install_all_headers(EXCLUDE_DIRS detail) -install(DIRECTORY detail DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}/ignition/${GZ_DESIGNATION}) +install(DIRECTORY detail DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}/gz/${GZ_DESIGNATION}) diff --git a/include/gz/math/Capsule.hh b/include/gz/math/Capsule.hh new file mode 100644 index 000000000..1af18d89e --- /dev/null +++ b/include/gz/math/Capsule.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2020 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_CAPSULE_HH_ +#define GZ_MATH_CAPSULE_HH_ + +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" + +namespace ignition +{ + namespace math + { + // Foward declarations + class CapsulePrivate; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Capsule Capsule.hh ignition/math/Capsule.hh + /// \brief A representation of a capsule or sphere-capped cylinder. + /// + /// The capsule class supports defining a capsule with a radius, + /// length, and material properties. The shape is equivalent to a cylinder + /// aligned with the Z-axis and capped with hemispheres. Radius and + /// length are in meters. See Material for more on material properties. + /// \tparam Precision Scalar numeric type. + template + class Capsule + { + /// \brief Default constructor. The default radius and length are both + /// zero. + public: Capsule() = default; + + /// \brief Construct a capsule with a length and radius. + /// \param[in] _length Length of the capsule. + /// \param[in] _radius Radius of the capsule. + public: Capsule(const Precision _length, const Precision _radius); + + /// \brief Construct a capsule with a length, radius, and material. + /// \param[in] _length Length of the capsule. + /// \param[in] _radius Radius of the capsule. + /// \param[in] _mat Material property for the capsule. + public: Capsule(const Precision _length, const Precision _radius, + const Material &_mat); + + /// \brief Get the radius in meters. + /// \return The radius of the capsule in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the capsule in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the length in meters. + /// \return The length of the capsule in meters. + public: Precision Length() const; + + /// \brief Set the length in meters. + /// \param[in] _length The length of the capsule in meters. + public: void SetLength(const Precision _length); + + /// \brief Get the material associated with this capsule. + /// \return The material assigned to this capsule + public: const Material &Mat() const; + + /// \brief Set the material associated with this capsule. + /// \param[in] _mat The material assigned to this capsule + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this capsule. This function + /// is only meaningful if the capsule's radius, length, and material + /// have been set. + /// \return The computed mass matrix if parameters are valid + /// (radius > 0), (length > 0), and (density > 0). Otherwise + /// std::nullopt is returned. + public: std::optional< MassMatrix3 > MassMatrix() const; + + /// \brief Check if this capsule is equal to the provided capsule. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Capsule &_capsule) const; + + /// \brief Get the volume of the capsule in m^3. + /// \return Volume of the capsule in m^3. + public: Precision Volume() const; + + /// \brief Compute the capsule's density given a mass value. The + /// capsule is assumed to be solid with uniform density. This + /// function requires the capsule's radius and length to be set to + /// values greater than zero. The Material of the capsule is ignored. + /// \param[in] _mass Mass of the capsule, in kg. This value should be + /// greater than zero. + /// \return Density of the capsule in kg/m^3. A NaN is returned + /// if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this capsule based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// capsule is assumed to be solid with uniform density. This + /// function requires the capsule's radius and length to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the capsule, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// capsule's radius, length, or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the capsule. + private: Precision radius = 0.0; + + /// \brief Length of the capsule. + private: Precision length = 0.0; + + /// \brief the capsule's material. + private: Material material; + }; + + /// \typedef Capsule Capsulei + /// \brief Capsule with integer precision. + typedef Capsule Capsulei; + + /// \typedef Capsule Capsuled + /// \brief Capsule with double precision. + typedef Capsule Capsuled; + + /// \typedef Capsule Capsulef + /// \brief Capsule with float precision. + typedef Capsule Capsulef; + } + } +} +#include "gz/math/detail/Capsule.hh" + +#endif diff --git a/include/gz/math/Color.hh b/include/gz/math/Color.hh new file mode 100644 index 000000000..9735edc3a --- /dev/null +++ b/include/gz/math/Color.hh @@ -0,0 +1,362 @@ +/* + * Copyright (C) 2017 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_COLOR_HH_ +#define GZ_MATH_COLOR_HH_ + +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Color Color.hh ignition/math/Color.hh + /// \brief Defines a color using a red (R), green (G), blue (B), and alpha + /// (A) component. Each color component is in the range [0..1]. + /// + /// ## Example + /// + /// \snippet examples/color_example.cc complete + class IGNITION_MATH_VISIBLE Color + { + /// \brief (1, 1, 1) + public: static const Color White; + /// \brief (0, 0, 0) + public: static const Color Black; + /// \brief (1, 0, 0) + public: static const Color Red; + /// \brief (0, 1, 0) + public: static const Color Green; + /// \brief (0, 0, 1) + public: static const Color Blue; + /// \brief (1, 1, 0) + public: static const Color Yellow; + /// \brief (1, 0, 1) + public: static const Color Magenta; + /// \brief (0, 1, 1) + public: static const Color Cyan; + + /// \typedef RGBA + /// \brief A RGBA packed value as an unsigned int + /// Each 8 bits corresponds to a channel. + /// + /// \code + /// RGBA a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red. + /// \endcode + public: typedef unsigned int RGBA; + + /// \typedef BGRA + /// \brief A BGRA packed value as an unsigned int + /// Each 8 bits corresponds to a channel. + /// + /// \code + /// BGRA a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue. + /// \endcode + public: typedef unsigned int BGRA; + + /// \typedef ARGB + /// \brief A ARGB packed value as an unsigned int + /// Each 8 bits corresponds to a channel. + /// + /// \code + /// ARGB a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue. + /// \endcode + public: typedef unsigned int ARGB; + + /// \typedef ABGR + /// \brief A ABGR packed value as an unsigned int + /// Each 8 bits corresponds to a channel. + /// + /// \code + /// ABGR a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red. + /// \endcode + public: typedef unsigned int ABGR; + + /// \brief Constructor + public: Color(); + + /// \brief Constructor + /// \param[in] _r Red value (range 0 to 1) + /// \param[in] _g Green value (range 0 to 1 + /// \param[in] _b Blue value (range 0 to 1 + /// \param[in] _a Alpha value (0=transparent, 1=opaque) + public: Color(const float _r, const float _g, const float _b, + const float _a = 1.0); + + /// \brief Copy Constructor + /// \param[in] _clr Color to copy + public: Color(const Color &_clr); + + /// \brief Destructor + public: virtual ~Color(); + + /// \brief Reset the color to default values to red=0, green=0, + /// blue=0, alpha=1. + public: void Reset(); + + /// \brief Set the contents of the vector + /// \param[in] _r Red value (range 0 to 1) + /// \param[in] _g Green value (range 0 to 1) + /// \param[in] _b Blue value (range 0 to 1) + /// \param[in] _a Alpha value (0=transparent, 1=opaque) + public: void Set(const float _r = 1, const float _g = 1, + const float _b = 1, const float _a = 1); + + /// \brief Get the color in HSV colorspace + /// \return HSV values in a Vector3f format. A vector3f containing + /// {NAN_F, NAN_F, NAN_F} is returned on error. + public: Vector3f HSV() const; + + /// \brief Set a color based on HSV values + /// \param[in] _h Hue(0..360) + /// \param[in] _s Saturation(0..1) + /// \param[in] _v Value(0..1) + public: void SetFromHSV(const float _h, const float _s, const float _v); + + /// \brief Get the color in YUV colorspace + /// \return the YUV color + public: Vector3f YUV() const; + + /// \brief Set from yuv + /// \param[in] _y value + /// \param[in] _u value + /// \param[in] _v value + public: void SetFromYUV(const float _y, const float _u, const float _v); + + /// \brief Equal operator + /// \param[in] _pt Color to copy + /// \return Reference to this color + public: Color &operator=(const Color &_pt); + + /// \brief Array index operator + /// \param[in] _index Color component index(0=red, 1=green, 2=blue) + /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is + /// returned if the _index is invalid + public: float operator[](const unsigned int _index); + + /// \brief Get as uint32 RGBA packed value + /// \return the color + public: RGBA AsRGBA() const; + + /// \brief Get as uint32 BGRA packed value + /// \return the color + public: BGRA AsBGRA() const; + + /// \brief Get as uint32 ARGB packed value + /// \return the color + public: ARGB AsARGB() const; + + /// \brief Get as uint32 ABGR packed value + /// \return the color + public: ABGR AsABGR() const; + + /// \brief Set from uint32 RGBA packed value + /// \param[in] _v the new color + public: void SetFromRGBA(const RGBA _v); + + /// \brief Set from uint32 BGRA packed value + /// \param[in] _v the new color + public: void SetFromBGRA(const BGRA _v); + + /// \brief Set from uint32 ARGB packed value + /// \param[in] _v the new color + public: void SetFromARGB(const ARGB _v); + + /// \brief Set from uint32 ABGR packed value + /// \param[in] _v the new color + public: void SetFromABGR(const ABGR _v); + + /// \brief Addition operator (this + _pt) + /// \param[in] _pt Color to add + /// \return The resulting color + public: Color operator+(const Color &_pt) const; + + /// \brief Add _v to all color components + /// \param[in] _v Value to add to each color component + /// \return The resulting color + public: Color operator+(const float &_v) const; + + /// \brief Addition equal operator + /// \param[in] _pt Color to add + /// \return The resulting color + public: const Color &operator+=(const Color &_pt); + + /// \brief Subtraction operator + /// \param[in] _pt The color to substract + /// \return The resulting color + public: Color operator-(const Color &_pt) const; + + /// \brief Subtract _v from all color components + /// \param[in] _v Value to subtract + /// \return The resulting color + public: Color operator-(const float &_v) const; + + /// \brief Subtraction equal operator + /// \param[in] _pt Color to subtract + /// \return The resulting color + public: const Color &operator-=(const Color &_pt); + + /// \brief Division operator + /// \param[in] _pt Color to divide by + /// \return The resulting color + public: const Color operator/(const Color &_pt) const; + + /// \brief Divide all color component by _v + /// \param[in] _v The value to divide by + /// \return The resulting color + public: const Color operator/(const float &_v) const; + + /// \brief Division equal operator + /// \param[in] _pt Color to divide by + /// \return The resulting color + public: const Color &operator/=(const Color &_pt); + + /// \brief Multiplication operator + /// \param[in] _pt The color to muliply by + /// \return The resulting color + public: const Color operator*(const Color &_pt) const; + + /// \brief Multiply all color components by _v + /// \param[in] _v The value to multiply by + /// \return The resulting color + public: const Color operator*(const float &_v) const; + + /// \brief Multiplication equal operator + /// \param[in] _pt The color to muliply by + /// \return The resulting color + public: const Color &operator*=(const Color &_pt); + + /// \brief Equality operator + /// \param[in] _pt The color to check for equality + /// \return True if the this color equals _pt + public: bool operator==(const Color &_pt) const; + + /// \brief Inequality operator + /// \param[in] _pt The color to check for inequality + /// \return True if the this color does not equal _pt + public: bool operator!=(const Color &_pt) const; + + /// \brief Clamp the color values to valid ranges + private: void Clamp(); + + /// \brief Stream insertion operator + /// \param[in] _out the output stream + /// \param[in] _pt the color + /// \return the output stream + public: friend std::ostream &operator<<(std::ostream &_out, + const Color &_pt) + { + _out << _pt.r << " " << _pt.g << " " << _pt.b << " " << _pt.a; + return _out; + } + + /// \brief Stream insertion operator + /// \param[in] _in the input stream. If the input stream does not include + /// an alpha value, a default alpha value of 1.0 will be used. + /// \param[in] _pt + public: friend std::istream &operator>> (std::istream &_in, Color &_pt) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + _in >> _pt.r >> _pt.g >> _pt.b; + // Since alpha is optional, check if it's there before parsing + while (_in.good() && std::isspace(_in.peek())) + { + _in.get(); + } + if (_in.good()) + { + _in >> _pt.a; + } + else if (!_in.fail()) + { + _pt.a = 1.0; + } + return _in; + } + + /// \brief Get the red value + /// \return The red value + public: float R() const; + + /// \brief Get the green value + /// \return The green value + public: float G() const; + + /// \brief Get the blue value + /// \return The blue value + public: float B() const; + + /// \brief Get the alpha value + /// \return The alpha value + public: float A() const; + + /// \brief Get a mutable reference to the red value + /// \return The red value + public: float &R(); + + /// \brief Get a mutable reference to the green value + /// \return The green value + public: float &G(); + + /// \brief Get a mutable reference to the blue value + /// \return The blue value + public: float &B(); + + /// \brief Get a mutable reference to the alpha value + /// \return The alpha value + public: float &A(); + + /// \brief Set the red value + /// \param[in] _r New red value + public: void R(const float _r); + + /// \brief Set the green value + /// \param[in] _g New green value + public: void G(const float _g); + + /// \brief Set the blue value + /// \param[in] _b New blue value + public: void B(const float _b); + + /// \brief Set the alpha value + /// \param[in] _a New alpha value + public: void A(const float _a); + + /// \brief Red value + private: float r = 0; + + /// \brief Green value + private: float g = 0; + + /// \brief Blue value + private: float b = 0; + + /// \brief Alpha value + private: float a = 1; + }; + } + } +} +#endif diff --git a/include/gz/math/Cylinder.hh b/include/gz/math/Cylinder.hh new file mode 100644 index 000000000..9ebfc5d15 --- /dev/null +++ b/include/gz/math/Cylinder.hh @@ -0,0 +1,183 @@ +/* + * Copyright (C) 2018 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_CYLINDER_HH_ +#define GZ_MATH_CYLINDER_HH_ + +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" + +namespace ignition +{ + namespace math + { + // Foward declarations + class CylinderPrivate; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Cylinder Cylinder.hh ignition/math/Cylinder.hh + /// \brief A representation of a cylinder. + /// + /// The cylinder class supports defining a cylinder with a radius, + /// length, rotational offset, and material properties. Radius and + /// length are in meters. See Material for more on material properties. + /// By default, a cylinder's length is aligned with the Z axis. The + /// rotational offset encodes a rotation from the z axis. + template + class Cylinder + { + /// \brief Default constructor. The default radius and length are both + /// zero. The default rotational offset is + /// Quaternion::Identity. + public: Cylinder() = default; + + /// \brief Construct a cylinder with a length, radius, and optionally + /// a rotational offset. + /// \param[in] _length Length of the cylinder. + /// \param[in] _radius Radius of the cylinder. + /// \param[in] _rotOffset Rotational offset of the cylinder. + public: Cylinder(const Precision _length, const Precision _radius, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Construct a cylinder with a length, radius, material and + /// optionally a rotational offset. + /// \param[in] _length Length of the cylinder. + /// \param[in] _radius Radius of the cylinder. + /// \param[in] _mat Material property for the cylinder. + /// \param[in] _rotOffset Rotational offset of the cylinder. + public: Cylinder(const Precision _length, const Precision _radius, + const Material &_mat, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Destructor + public: ~Cylinder() = default; + + /// \brief Get the radius in meters. + /// \return The radius of the cylinder in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the cylinder in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the length in meters. + /// \return The length of the cylinder in meters. + public: Precision Length() const; + + /// \brief Set the length in meters. + /// \param[in] _length The length of the cylinder in meters. + public: void SetLength(const Precision _length); + + /// \brief Get the rotational offset. By default, a cylinder's length + /// is aligned with the Z axis. The rotational offset encodes + /// a rotation from the z axis. + /// \return The cylinder's rotational offset. + /// \sa void SetRotationalOffset(const Quaternion &_rot) + public: Quaternion RotationalOffset() const; + + /// \brief Set the rotation offset. + /// See Quaternion RotationalOffset() for details on the + /// rotational offset. + /// \sa Quaternion RotationalOffset() const + public: void SetRotationalOffset( + const Quaternion &_rotOffset); + + /// \brief Get the material associated with this cylinder. + /// \return The material assigned to this cylinder + public: const Material &Mat() const; + + /// \brief Set the material associated with this cylinder. + /// \param[in] _mat The material assigned to this cylinder + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this cylinder. This function + /// is only meaningful if the cylinder's radius, length, and material + /// have been set. Optionally, set the rotational offset. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid radius (<=0), length (<=0), or density + /// (<=0). + public: bool MassMatrix(MassMatrix3d &_massMat) const; + + /// \brief Check if this cylinder is equal to the provided cylinder. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Cylinder &_cylinder) const; + + /// \brief Get the volume of the cylinder in m^3. + /// \return Volume of the cylinder in m^3. + public: Precision Volume() const; + + /// \brief Compute the cylinder's density given a mass value. The + /// cylinder is assumed to be solid with uniform density. This + /// function requires the cylinder's radius and length to be set to + /// values greater than zero. The Material of the cylinder is ignored. + /// \param[in] _mass Mass of the cylinder, in kg. This value should be + /// greater than zero. + /// \return Density of the cylinder in kg/m^3. A negative value is + /// returned if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this cylinder based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// cylinder is assumed to be solid with uniform density. This + /// function requires the cylinder's radius and length to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the cylinder, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// cylinder's radius, length, or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the cylinder. + private: Precision radius = 0.0; + + /// \brief Length of the cylinder. + private: Precision length = 0.0; + + /// \brief the cylinder's material. + private: Material material; + + /// \brief Rotational offset. + private: Quaternion rotOffset = + Quaternion::Identity; + }; + + /// \typedef Cylinder Cylinderi + /// \brief Cylinder with integer precision. + typedef Cylinder Cylinderi; + + /// \typedef Cylinder Cylinderd + /// \brief Cylinder with double precision. + typedef Cylinder Cylinderd; + + /// \typedef Cylinder Cylinderf + /// \brief Cylinder with float precision. + typedef Cylinder Cylinderf; + } + } +} +#include "gz/math/detail/Cylinder.hh" + +#endif diff --git a/include/gz/math/DiffDriveOdometry.hh b/include/gz/math/DiffDriveOdometry.hh new file mode 100644 index 000000000..2891d4c78 --- /dev/null +++ b/include/gz/math/DiffDriveOdometry.hh @@ -0,0 +1,155 @@ +/* + * 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. + * +*/ +#ifndef GZ_MATH_DIFFDRIVEODOMETRY_HH_ +#define GZ_MATH_DIFFDRIVEODOMETRY_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Use a steady clock + using clock = std::chrono::steady_clock; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declarations. + class DiffDriveOdometryPrivate; + + /** \class DiffDriveOdometry DiffDriveOdometry.hh \ + * ignition/math/DiffDriveOdometry.hh + **/ + /// \brief Computes odometry values based on a set of kinematic + /// properties and wheel speeds for a diff-drive vehicle. + /// + /// A vehicle with a heading of zero degrees has a local + /// reference frame according to the diagram below. + /// + /// Y + /// ^ + /// | + /// | + /// O--->X(forward) + /// + /// Rotating the right wheel while keeping the left wheel fixed will cause + /// the vehicle to rotate counter-clockwise. For example (excuse the + /// lack of precision with ASCII art): + /// + /// Y X(forward) + /// ^ ^ + /// \ / + /// \ / + /// O + /// + /// **Example Usage** + /// + /// \code{.cpp} + /// gz::math::DiffDriveOdometry odom; + /// odom.SetWheelParams(2.0, 0.5, 0.5); + /// odom.Init(std::chrono::steady_clock::now()); + /// + /// // ... Some time later + /// + /// // Both wheels have rotated the same amount + /// odom.Update(IGN_DTOR(2), IGN_DTOR(2), std::chrono::steady_clock::now()); + /// + /// // ... Some time later + /// + /// // The left wheel has rotated, the right wheel did not rotate + /// odom.Update(IGN_DTOR(4), IGN_DTOR(2), std::chrono::steady_clock::now()); + /// \endcode + class IGNITION_MATH_VISIBLE DiffDriveOdometry + { + /// \brief Constructor. + /// \param[in] _windowSize Rolling window size used to compute the + /// velocity mean + public: explicit DiffDriveOdometry(size_t _windowSize = 10); + + /// \brief Destructor. + public: ~DiffDriveOdometry(); + + /// \brief Initialize the odometry + /// \param[in] _time Current time. + public: void Init(const clock::time_point &_time); + + /// \brief Get whether Init has been called. + /// \return True if Init has been called, false otherwise. + public: bool Initialized() const; + + /// \brief Updates the odometry class with latest wheels and + /// steerings position + /// \param[in] _leftPos Left wheel position in radians. + /// \param[in] _rightPos Right wheel postion in radians. + /// \param[in] _time Current time point. + /// \return True if the odometry is actually updated. + public: bool Update(const Angle &_leftPos, const Angle &_rightPos, + const clock::time_point &_time); + + /// \brief Get the heading. + /// \return The heading in radians. + public: const Angle &Heading() const; + + /// \brief Get the X position. + /// \return The X position in meters + public: double X() const; + + /// \brief Get the Y position. + /// \return The Y position in meters. + public: double Y() const; + + /// \brief Get the linear velocity. + /// \return The linear velocity in meter/second. + public: double LinearVelocity() const; + + /// \brief Get the angular velocity. + /// \return The angular velocity in radian/second. + public: const Angle &AngularVelocity() const; + + /// \brief Set the wheel parameters including the radius and separation. + /// \param[in] _wheelSeparation Distance between left and right wheels. + /// \param[in] _leftWheelRadius Radius of the left wheel. + /// \param[in] _rightWheelRadius Radius of the right wheel. + public: void SetWheelParams(double _wheelSeparation, + double _leftWheelRadius, double _rightWheelRadius); + + /// \brief Set the velocity rolling window size. + /// \param[in] _size The Velocity rolling window size. + public: void SetVelocityRollingWindowSize(size_t _size); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} + +#endif diff --git a/include/gz/math/Ellipsoid.hh b/include/gz/math/Ellipsoid.hh new file mode 100644 index 000000000..cf11a86ca --- /dev/null +++ b/include/gz/math/Ellipsoid.hh @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2020 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_ELLIPSOID_HH_ +#define GZ_MATH_ELLIPSOID_HH_ + +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Ellipsoid Ellipsoid.hh ignition/math/Ellipsoid.hh + /// \brief A representation of a general ellipsoid. + /// + /// The ellipsoid class supports defining a ellipsoid with three radii and + /// material properties. Radii are in meters. See Material for more on + /// material properties. + /// \tparam Precision Scalar numeric type. + template + class Ellipsoid + { + /// \brief Default constructor. The default radius and length are both + /// zero. + public: Ellipsoid() = default; + + /// \brief Construct a ellipsoid with a Vector3 of three radii. + /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid + public: explicit Ellipsoid(const Vector3 &_radii); + + /// \brief Construct a ellipsoid with three radii and a material. + /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid + /// \param[in] _mat Material property for the ellipsoid. + public: Ellipsoid(const Vector3 &_radii, + const Material &_mat); + + /// \brief Get the radius in meters. + /// \return The radius of the ellipsoid in meters. + public: Vector3 Radii() const; + + /// \brief Set the radius in meters. + /// \param[in] _radii The radii of the ellipsoid in meters. + public: void SetRadii(const Vector3 &_radii); + + /// \brief Get the material associated with this ellipsoid. + /// \return The material assigned to this ellipsoid + public: const Material &Mat() const; + + /// \brief Set the material associated with this ellipsoid. + /// \param[in] _mat The material assigned to this ellipsoid + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this ellipsoid. This function + /// is only meaningful if the ellipsoid's radii and material + /// have been set. + /// \return The computed mass matrix if parameters are valid + /// (radius > 0), (length > 0), and (density > 0). Otherwise + /// std::nullopt is returned. + public: std::optional< MassMatrix3 > MassMatrix() const; + + /// \brief Check if this ellipsoid is equal to the provided ellipsoid. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Ellipsoid &_ellipsoid) const; + + /// \brief Get the volume of the ellipsoid in m^3. + /// \return Volume of the ellipsoid in m^3. + public: Precision Volume() const; + + /// \brief Compute the ellipsoid's density given a mass value. The + /// ellipsoid is assumed to be solid with uniform density. This + /// function requires the ellipsoid's radius and length to be set to + /// values greater than zero. The Material of the ellipsoid is ignored. + /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be + /// greater than zero. + /// \return Density of the ellipsoid in kg/m^3. A NaN is returned + /// if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this ellipsoid based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// ellipsoid is assumed to be solid with uniform density. This + /// function requires the ellipsoid's radius and length to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// ellipsoid's radius, length, or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the ellipsoid. + private: Vector3 radii = Vector3::Zero; + + /// \brief the ellipsoid's material. + private: Material material; + }; + + /// \typedef Ellipsoid Ellipsoidi + /// \brief Ellipsoid with integer precision. + typedef Ellipsoid Ellipsoidi; + + /// \typedef Ellipsoid Ellipsoidd + /// \brief Ellipsoid with double precision. + typedef Ellipsoid Ellipsoidd; + + /// \typedef Ellipsoid Ellipsoidf + /// \brief Ellipsoid with float precision. + typedef Ellipsoid Ellipsoidf; + } + } +} +#include "gz/math/detail/Ellipsoid.hh" + +#endif diff --git a/include/gz/math/Filter.hh b/include/gz/math/Filter.hh new file mode 100644 index 000000000..8bca55dc0 --- /dev/null +++ b/include/gz/math/Filter.hh @@ -0,0 +1,252 @@ +/* + * Copyright (C) 2014 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_FILTER_HH_ +#define GZ_MATH_FILTER_HH_ + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Filter Filter.hh ignition/math/Filter.hh + /// \brief Filter base class + template + class Filter + { + /// \brief Destructor. + public: virtual ~Filter() {} + + /// \brief Set the output of the filter. + /// \param[in] _val New value. + public: virtual void Set(const T &_val) + { + y0 = _val; + } + + /// \brief Set the cutoff frequency and sample rate. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: virtual void Fc(double _fc, double _fs) = 0; + + /// \brief Get the output of the filter. + /// \return Filter's output. + public: virtual const T &Value() const + { + return y0; + } + + /// \brief Output. + protected: T y0{}; + }; + + /// \class OnePole Filter.hh ignition/math/Filter.hh + /// \brief A one-pole DSP filter. + /// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/ + template + class OnePole : public Filter + { + /// \brief Constructor. + public: OnePole() = default; + + /// \brief Constructor. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: OnePole(double _fc, double _fs) + { + this->Fc(_fc, _fs); + } + + // Documentation Inherited. + public: virtual void Fc(double _fc, double _fs) override + { + b1 = exp(-2.0 * IGN_PI * _fc / _fs); + a0 = 1.0 - b1; + } + + /// \brief Update the filter's output. + /// \param[in] _x Input value. + /// \return The filter's current output. + public: const T& Process(const T &_x) + { + this->y0 = a0 * _x + b1 * this->y0; + return this->y0; + } + + /// \brief Input gain control. + protected: double a0 = 0; + + /// \brief Gain of the feedback. + protected: double b1 = 0; + }; + + /// \class OnePoleQuaternion Filter.hh ignition/math/Filter.hh + /// \brief One-pole quaternion filter. + class OnePoleQuaternion : public OnePole + { + /// \brief Constructor. + public: OnePoleQuaternion() + { + this->Set(math::Quaterniond(1, 0, 0, 0)); + } + + /// \brief Constructor. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: OnePoleQuaternion(double _fc, double _fs) + : OnePole(_fc, _fs) + { + this->Set(math::Quaterniond(1, 0, 0, 0)); + } + + /// \brief Update the filter's output. + /// \param[in] _x Input value. + /// \return The filter's current output. + public: const math::Quaterniond& Process( + const math::Quaterniond &_x) + { + y0 = math::Quaterniond::Slerp(a0, y0, _x); + return y0; + } + }; + + /// \class OnePoleVector3 Filter.hh ignition/math/Filter.hh + /// \brief One-pole vector3 filter. + class OnePoleVector3 : public OnePole + { + /// \brief Constructor. + public: OnePoleVector3() + { + this->Set(math::Vector3d(0, 0, 0)); + } + + /// \brief Constructor. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: OnePoleVector3(double _fc, double _fs) + : OnePole(_fc, _fs) + { + this->Set(math::Vector3d(0, 0, 0)); + } + }; + + /// \class BiQuad Filter.hh ignition/math/Filter.hh + /// \brief Bi-quad filter base class. + /// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/ + template + class BiQuad : public Filter + { + /// \brief Constructor. + public: BiQuad() = default; + + /// \brief Constructor. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: BiQuad(double _fc, double _fs) + { + this->Fc(_fc, _fs); + } + + // Documentation Inherited. + public: void Fc(double _fc, double _fs) override + { + this->Fc(_fc, _fs, 0.5); + } + + /// \brief Set the cutoff frequency, sample rate and Q coefficient. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + /// \param[in] _q Q coefficient. + public: void Fc(double _fc, double _fs, double _q) + { + double k = tan(IGN_PI * _fc / _fs); + double kQuadDenom = k * k + k / _q + 1.0; + this->a0 = k * k/ kQuadDenom; + this->a1 = 2 * this->a0; + this->a2 = this->a0; + this->b0 = 1.0; + this->b1 = 2 * (k * k - 1.0) / kQuadDenom; + this->b2 = (k * k - k / _q + 1.0) / kQuadDenom; + } + + /// \brief Set the current filter's output. + /// \param[in] _val New filter's output. + public: virtual void Set(const T &_val) override + { + this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val; + } + + /// \brief Update the filter's output. + /// \param[in] _x Input value. + /// \return The filter's current output. + public: virtual const T& Process(const T &_x) + { + this->y0 = this->a0 * _x + + this->a1 * this->x1 + + this->a2 * this->x2 - + this->b1 * this->y1 - + this->b2 * this->y2; + + this->x2 = this->x1; + this->x1 = _x; + this->y2 = this->y1; + this->y1 = this->y0; + return this->y0; + } + + /// \brief Input gain control coefficients. + protected: double a0 = 0, + a1 = 0, + a2 = 0, + b0 = 0, + b1 = 0, + b2 = 0; + + /// \brief Gain of the feedback coefficients. + protected: T x1{}, x2{}, y1{}, y2{}; + }; + + /// \class BiQuadVector3 Filter.hh ignition/math/Filter.hh + /// \brief BiQuad vector3 filter + class BiQuadVector3 : public BiQuad + { + /// \brief Constructor. + public: BiQuadVector3() + { + this->Set(math::Vector3d(0, 0, 0)); + } + + /// \brief Constructor. + /// \param[in] _fc Cutoff frequency. + /// \param[in] _fs Sample rate. + public: BiQuadVector3(double _fc, double _fs) + : BiQuad(_fc, _fs) + { + this->Set(math::Vector3d(0, 0, 0)); + } + }; + } + } +} + +#endif diff --git a/include/gz/math/Frustum.hh b/include/gz/math/Frustum.hh new file mode 100644 index 000000000..c42fae6f4 --- /dev/null +++ b/include/gz/math/Frustum.hh @@ -0,0 +1,187 @@ +/* + * Copyright (C) 2015 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_FRUSTUM_HH_ +#define GZ_MATH_FRUSTUM_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declaration of private data + class FrustumPrivate; + + /// \brief Mathematical representation of a frustum and related functions. + /// This is also known as a view frustum. + class IGNITION_MATH_VISIBLE Frustum + { + /// \brief Planes that define the boundaries of the frustum. + public: enum FrustumPlane + { + /// \brief Near plane + FRUSTUM_PLANE_NEAR = 0, + + /// \brief Far plane + FRUSTUM_PLANE_FAR = 1, + + /// \brief Left plane + FRUSTUM_PLANE_LEFT = 2, + + /// \brief Right plane + FRUSTUM_PLANE_RIGHT = 3, + + /// \brief Top plane + FRUSTUM_PLANE_TOP = 4, + + /// \brief Bottom plane + FRUSTUM_PLANE_BOTTOM = 5 + }; + + /// \brief Default constructor. With the following default values: + /// + /// * near: 0.0 + /// * far: 1.0 + /// * fov: 0.78539 radians (45 degrees) + /// * aspect ratio: 1.0 + /// * pose: Pose3d::Zero + public: Frustum(); + + /// \brief Constructor + /// \param[in] _near Near plane distance. This is the distance from + /// the frustum's vertex to the closest plane + /// \param[in] _far Far plane distance. This is the distance from the + /// frustum's vertex to the farthest plane. + /// \param[in] _fov Field of view. The field of view is the + /// angle between the frustum's vertex and the edges of the near or far + /// plane. This value represents the horizontal angle. + /// \param[in] _aspectRatio The aspect ratio, which is the width divided + /// by height of the near or far planes. + /// \param[in] _pose Pose of the frustum, which is the vertex (top of + /// the pyramid). + public: Frustum(const double _near, + const double _far, + const math::Angle &_fov, + const double _aspectRatio, + const math::Pose3d &_pose = math::Pose3d::Zero); + + /// \brief Copy Constructor + /// \param[in] _p Frustum to copy. + public: Frustum(const Frustum &_p); + + /// \brief Destructor + public: virtual ~Frustum(); + + /// \brief Get the near distance. This is the distance from the + /// frustum's vertex to the closest plane. + /// \return Near distance. + /// \sa SetNear + public: double Near() const; + + /// \brief Set the near distance. This is the distance from the + /// frustum's vertex to the closest plane. + /// \param[in] _near Near distance. + /// \sa Near + public: void SetNear(const double _near); + + /// \brief Get the far distance. This is the distance from the + /// frustum's vertex to the farthest plane. + /// \return Far distance. + /// \sa SetFar + public: double Far() const; + + /// \brief Set the far distance. This is the distance from the + /// frustum's vertex to the farthest plane. + /// \param[in] _far Far distance. + /// \sa Far + public: void SetFar(const double _far); + + /// \brief Get the horizontal field of view. The field of view is the + /// angle between the frustum's vertex and the edges of the near or far + /// plane. This value represents the horizontal angle. + /// \return The field of view. + /// \sa SetFOV + public: math::Angle FOV() const; + + /// \brief Set the horizontal field of view. The field of view is the + /// angle between the frustum's vertex and the edges of the near or far + /// plane. This value represents the horizontal angle. + /// \param[in] _fov The field of view. + /// \sa FOV + public: void SetFOV(const math::Angle &_fov); + + /// \brief Get the aspect ratio, which is the width divided by height + /// of the near or far planes. + /// \return The frustum's aspect ratio. + /// \sa SetAspectRatio + public: double AspectRatio() const; + + /// \brief Set the aspect ratio, which is the width divided by height + /// of the near or far planes. + /// \param[in] _aspectRatio The frustum's aspect ratio. + /// \sa AspectRatio + public: void SetAspectRatio(const double _aspectRatio); + + /// \brief Get a plane of the frustum. + /// \param[in] _plane The plane to return. + /// \return Plane of the frustum. + public: Planed Plane(const FrustumPlane _plane) const; + + /// \brief Check if a box lies inside the pyramid frustum. + /// \param[in] _b Box to check. + /// \return True if the box is inside the pyramid frustum. + public: bool Contains(const AxisAlignedBox &_b) const; + + /// \brief Check if a point lies inside the pyramid frustum. + /// \param[in] _p Point to check. + /// \return True if the point is inside the pyramid frustum. + public: bool Contains(const Vector3d &_p) const; + + /// \brief Get the pose of the frustum + /// \return Pose of the frustum + /// \sa SetPose + public: Pose3d Pose() const; + + /// \brief Set the pose of the frustum + /// \param[in] _pose Pose of the frustum, top vertex. + /// \sa Pose + public: void SetPose(const Pose3d &_pose); + + /// \brief Assignment operator. Set this frustum to the parameter. + /// \param[in] _f Frustum to copy + /// \return The new frustum. + public: Frustum &operator=(const Frustum &_f); + + /// \brief Compute the planes of the frustum. This is called whenever + /// a property of the frustum is changed. + private: void ComputePlanes(); + + /// \internal + /// \brief Private data pointer + private: FrustumPrivate *dataPtr; + }; + } + } +} +#endif diff --git a/include/gz/math/GaussMarkovProcess.hh b/include/gz/math/GaussMarkovProcess.hh new file mode 100644 index 000000000..eef0de7ff --- /dev/null +++ b/include/gz/math/GaussMarkovProcess.hh @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2020 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_GAUSSMARKOVPROCESS_HH_ +#define GZ_MATH_GAUSSMARKOVPROCESS_HH_ + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Use a steady clock + using clock = std::chrono::steady_clock; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declarations. + class GaussMarkovProcessPrivate; + + /** \class GaussMarkovProcess GaussMarkovProcess.hh\ + * ignition/math/GaussMarkovProcess.hh + **/ + /// \brief Implementation of a stationary gauss-markov process, also + /// known as a Ornstein Ulenbeck process. + /// + /// See the Update(const clock::duration &) for details on the forumla + /// used to update the process. + /// + /// ## Example usage + /// + /// \snippet examples/gauss_markov_process_example.cc complete + class IGNITION_MATH_VISIBLE GaussMarkovProcess + { + // Default constructor. This sets all the parameters to zero. + public: GaussMarkovProcess(); + + /// \brief Create a process with the provided process parameters. + /// This will also call Set(), and in turn Reset(). + /// \param[in] _start The start value of the process. + /// \param[in] _theta The theta (\f$\theta\f$) parameter. A value of + /// zero will be used if this parameter is negative. + /// \param[in] _mu The mu (\f$\mu\f$) parameter. + /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. A value of + /// zero will be used if this parameter is negative. + /// \sa Update(const clock::duration &) + public: GaussMarkovProcess(double _start, double _theta, double _mu, + double _sigma); + + /// \brief Destructor. + public: ~GaussMarkovProcess(); + + /// \brief Set the process parameters. This will also call Reset(). + /// \param[in] _start The start value of the process. + /// \param[in] _theta The theta (\f$\theta\f$) parameter. + /// \param[in] _mu The mu (\f$\mu\f$) parameter. + /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. + /// \sa Update(const clock::duration &) + public: void Set(double _start, double _theta, double _mu, double _sigma); + + /// \brief Get the start value. + /// \return The start value. + /// \sa Set(double, double, double, double) + public: double Start() const; + + /// \brief Get the current process value. + /// \return The value of the process. + public: double Value() const; + + /// \brief Get the theta (\f$\theta\f$) value. + /// \return The theta value. + /// \sa Set(double, double, double, double) + public: double Theta() const; + + /// \brief Get the mu (\f$\mu\f$) value. + /// \return The mu value. + /// \sa Set(double, double, double, double) + public: double Mu() const; + + /// \brief Get the sigma (\f$\sigma\f$) value. + /// \return The sigma value. + /// \sa Set(double, double, double, double) + public: double Sigma() const; + + /// \brief Reset the process. This will set the current process value + /// to the start value. + public: void Reset(); + + /// \brief Update the process and get the new value. + /// + /// The following equation is computed: + /// + /// \f$x_{t+1} += \theta * (\mu - x_t) * dt + \sigma * dW_t\f$ + /// + /// where + /// + /// * \f$\theta, \mu, \sigma\f$ are parameters specified by the + /// user. In order, the parameters are theta, mu, and sigma. Theta + /// and sigma must be greater than or equal to zero. You can think + /// of mu as representing the mean or equilibrium value, sigma as the + /// degree of volatility, and theta as the rate by which changes + /// dissipate and revert towards the mean. + /// * \f$dt\f$ is the time step in seconds. + /// * \f$dW_t\f$ is a random number drawm from a normal distribution + /// with mean of zero and variance of 1. + /// * \f$x_t\f$ is the current value of the Gauss-Markov process + /// * \f$x_{t+1}\f$ is the new value of the Gauss-Markvov process + /// + /// See also: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process + /// + /// This implementation include a drift parameter, mu. In financial + /// mathematics, this is known as a Vasicek model. + /// + /// \param[in] _dt Length of the timestep after which a new sample + /// should be taken. + /// \return The new value of this process. + public: double Update(const clock::duration &_dt); + + public: double Update(double _dt); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh new file mode 100644 index 000000000..202221978 --- /dev/null +++ b/include/gz/math/Helpers.hh @@ -0,0 +1,1174 @@ +/* + * Copyright (C) 2012 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_FUNCTIONS_HH_ +#define GZ_MATH_FUNCTIONS_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "gz/math/Export.hh" + +/// \brief The default tolerance value used by MassMatrix3::IsValid(), +/// MassMatrix3::IsPositive(), and MassMatrix3::ValidMoments() +template +constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); + +/// \brief Double maximum value. This value will be similar to 1.79769e+308 +/// \deprecated Use static const value instead. +#define IGN_DBL_MAX gz::math::DPRCT_MAX_D + +/// \brief Double min value. This value will be similar to 2.22507e-308 +/// \deprecated Use static const value instead. +#define IGN_DBL_MIN gz::math::DPRCT_MIN_D + +/// \brief Double low value, equivalent to -IGN_DBL_MAX +/// \deprecated Use static const value instead. +#define IGN_DBL_LOW gz::math::DPRCT_LOW_D + +/// \brief Double positive infinite value +/// \deprecated Use static const value instead. +#define IGN_DBL_INF gz::math::DPRCT_INF_D + +/// \brief Float maximum value. This value will be similar to 3.40282e+38 +/// \deprecated Use static const value instead. +#define IGN_FLT_MAX gz::math::DPRCT_MAX_F + +/// \brief Float minimum value. This value will be similar to 1.17549e-38 +/// \deprecated Use static const value instead. +#define IGN_FLT_MIN gz::math::DPRCT_MIN_F + +/// \brief Float lowest value, equivalent to -IGN_FLT_MAX +/// \deprecated Use static const value instead. +#define IGN_FLT_LOW gz::math::DPRCT_LOW_F + +/// \brief Float positive infinite value +/// \deprecated Use static const value instead. +#define IGN_FLT_INF gz::math::DPRCT_INF_F + +/// \brief 16bit unsigned integer maximum value +/// \deprecated Use static const value instead. +#define IGN_UINT16_MAX gz::math::DPRCT_MAX_UI16 + +/// \brief 16bit unsigned integer minimum value +/// \deprecated Use static const value instead. +#define IGN_UINT16_MIN gz::math::DPRCT_MIN_UI16 + +/// \brief 16bit unsigned integer lowest value. This is equivalent to +/// IGN_UINT16_MIN, and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_UINT16_LOW gz::math::DPRCT_LOW_UI16 + +/// \brief 16-bit unsigned integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_UINT16_INF gz::math::DPRCT_INF_UI16 + +/// \brief 16bit integer maximum value +/// \deprecated Use static const value instead. +#define IGN_INT16_MAX gz::math::DPRCT_MAX_I16 + +/// \brief 16bit integer minimum value +/// \deprecated Use static const value instead. +#define IGN_INT16_MIN gz::math::DPRCT_MIN_I16 + +/// \brief 16bit integer lowest value. This is equivalent to IGN_INT16_MIN, +/// and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_INT16_LOW gz::math::DPRCT_LOW_I16 + +/// \brief 16-bit integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_INT16_INF gz::math::DPRCT_INF_I16 + +/// \brief 32bit unsigned integer maximum value +/// \deprecated Use static const value instead. +#define IGN_UINT32_MAX gz::math::DPRCT_MAX_UI32 + +/// \brief 32bit unsigned integer minimum value +/// \deprecated Use static const value instead. +#define IGN_UINT32_MIN gz::math::DPRCT_MIN_UI32 + +/// \brief 32bit unsigned integer lowest value. This is equivalent to +/// IGN_UINT32_MIN, and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_UINT32_LOW gz::math::DPRCT_LOW_UI32 + +/// \brief 32-bit unsigned integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_UINT32_INF gz::math::DPRCT_INF_UI32 + +/// \brief 32bit integer maximum value +/// \deprecated Use static const value instead. +#define IGN_INT32_MAX gz::math::DPRCT_MAX_I32 + +/// \brief 32bit integer minimum value +/// \deprecated Use static const value instead. +#define IGN_INT32_MIN gz::math::DPRCT_MIN_I32 + +/// \brief 32bit integer minimum value. This is equivalent to IGN_INT32_MIN, +/// and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_INT32_LOW gz::math::DPRCT_LOW_I32 + +/// \brief 32-bit integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_INT32_INF gz::math::DPRCT_INF_I32 + +/// \brief 64bit unsigned integer maximum value +/// \deprecated Use static const value instead. +#define IGN_UINT64_MAX gz::math::DPRCT_MAX_UI64 + +/// \brief 64bit unsigned integer minimum value +/// \deprecated Use static const value instead. +#define IGN_UINT64_MIN gz::math::DPRCT_MIN_UI64 + +/// \brief 64bit unsigned integer lowest value. This is equivalent to +/// IGN_UINT64_MIN, and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_UINT64_LOW gz::math::DPRCT_LOW_UI64 + +/// \brief 64-bit unsigned integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_UINT64_INF gz::math::DPRCT_INF_UI64 + +/// \brief 64bit integer maximum value +/// \deprecated Use static const value instead. +#define IGN_INT64_MAX gz::math::DPRCT_MAX_I64 + +/// \brief 64bit integer minimum value +/// \deprecated Use static const value instead. +#define IGN_INT64_MIN gz::math::DPRCT_MIN_I64 + +/// \brief 64bit integer lowest value. This is equivalent to IGN_INT64_MIN, +/// and is defined here for completeness. +/// \deprecated Use static const value instead. +#define IGN_INT64_LOW gz::math::DPRCT_LOW_I64 + +/// \brief 64-bit integer positive infinite value +/// \deprecated Use static const value instead. +#define IGN_INT64_INF gz::math::DPRCT_INF_I64 + +/// \brief Define IGN_PI, IGN_PI_2, and IGN_PI_4. +/// This was put here for Windows support. +#ifdef M_PI +#define IGN_PI M_PI +#define IGN_PI_2 M_PI_2 +#define IGN_PI_4 M_PI_4 +#define IGN_SQRT2 M_SQRT2 +#else +#define IGN_PI 3.14159265358979323846 +#define IGN_PI_2 1.57079632679489661923 +#define IGN_PI_4 0.78539816339744830962 +#define IGN_SQRT2 1.41421356237309504880 +#endif + +/// \brief Define IGN_FP_VOLATILE for FP equality comparisons +/// Use volatile parameters when checking floating point equality on +/// the 387 math coprocessor to work around bugs from the 387 extra precision +#if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2 +#define IGN_FP_VOLATILE volatile +#else +#define IGN_FP_VOLATILE +#endif + +/// \brief Compute sphere volume +/// \param[in] _radius Sphere radius +#define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0) + +/// \brief Compute cylinder volume +/// \param[in] _r Cylinder base radius +/// \param[in] _l Cylinder length +#define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2)) + +/// \brief Compute box volume +/// \param[in] _x X length +/// \param[in] _y Y length +/// \param[in] _z Z length +#define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z) + +/// \brief Compute box volume from a vector +/// \param[in] _v Vector3d that contains the box's dimensions. +#define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z()) + +namespace ignition +{ + /// \brief Math classes and function useful in robot applications. + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \brief size_t type with a value of 0 + static const size_t IGN_ZERO_SIZE_T = 0u; + + /// \brief size_t type with a value of 1 + static const size_t IGN_ONE_SIZE_T = 1u; + + /// \brief size_t type with a value of 2 + static const size_t IGN_TWO_SIZE_T = 2u; + + /// \brief size_t type with a value of 3 + static const size_t IGN_THREE_SIZE_T = 3u; + + /// \brief size_t type with a value of 4 + static const size_t IGN_FOUR_SIZE_T = 4u; + + /// \brief size_t type with a value of 5 + static const size_t IGN_FIVE_SIZE_T = 5u; + + /// \brief size_t type with a value of 6 + static const size_t IGN_SIX_SIZE_T = 6u; + + /// \brief size_t type with a value of 7 + static const size_t IGN_SEVEN_SIZE_T = 7u; + + /// \brief size_t type with a value of 8 + static const size_t IGN_EIGHT_SIZE_T = 8u; + + /// \brief size_t type with a value of 9 + static const size_t IGN_NINE_SIZE_T = 9u; + + /// \brief Double maximum value. This value will be similar to 1.79769e+308 + static const double MAX_D = std::numeric_limits::max(); + + /// \brief Double min value. This value will be similar to 2.22507e-308 + static const double MIN_D = std::numeric_limits::min(); + + /// \brief Double low value, equivalent to -MAX_D + static const double LOW_D = std::numeric_limits::lowest(); + + /// \brief Double positive infinite value + static const double INF_D = std::numeric_limits::infinity(); + + /// \brief Returns the representation of a quiet not a number (NAN) + static const double NAN_D = std::numeric_limits::quiet_NaN(); + + /// \brief Float maximum value. This value will be similar to 3.40282e+38 + static const float MAX_F = std::numeric_limits::max(); + + /// \brief Float minimum value. This value will be similar to 1.17549e-38 + static const float MIN_F = std::numeric_limits::min(); + + /// \brief Float low value, equivalent to -MAX_F + static const float LOW_F = std::numeric_limits::lowest(); + + /// \brief float positive infinite value + static const float INF_F = std::numeric_limits::infinity(); + + /// \brief Returns the representation of a quiet not a number (NAN) + static const float NAN_F = std::numeric_limits::quiet_NaN(); + + /// \brief 16bit unsigned integer maximum value + static const uint16_t MAX_UI16 = std::numeric_limits::max(); + + /// \brief 16bit unsigned integer minimum value + static const uint16_t MIN_UI16 = std::numeric_limits::min(); + + /// \brief 16bit unsigned integer lowest value. This is equivalent to + /// IGN_UINT16_MIN, and is defined here for completeness. + static const uint16_t LOW_UI16 = std::numeric_limits::lowest(); + + /// \brief 16-bit unsigned integer positive infinite value + static const uint16_t INF_UI16 = std::numeric_limits::infinity(); + + /// \brief 16bit unsigned integer maximum value + static const int16_t MAX_I16 = std::numeric_limits::max(); + + /// \brief 16bit unsigned integer minimum value + static const int16_t MIN_I16 = std::numeric_limits::min(); + + /// \brief 16bit unsigned integer lowest value. This is equivalent to + /// IGN_INT16_MIN, and is defined here for completeness. + static const int16_t LOW_I16 = std::numeric_limits::lowest(); + + /// \brief 16-bit unsigned integer positive infinite value + static const int16_t INF_I16 = std::numeric_limits::infinity(); + + /// \brief 32bit unsigned integer maximum value + static const uint32_t MAX_UI32 = std::numeric_limits::max(); + + /// \brief 32bit unsigned integer minimum value + static const uint32_t MIN_UI32 = std::numeric_limits::min(); + + /// \brief 32bit unsigned integer lowest value. This is equivalent to + /// IGN_UINT32_MIN, and is defined here for completeness. + static const uint32_t LOW_UI32 = std::numeric_limits::lowest(); + + /// \brief 32-bit unsigned integer positive infinite value + static const uint32_t INF_UI32 = std::numeric_limits::infinity(); + + /// \brief 32bit unsigned integer maximum value + static const int32_t MAX_I32 = std::numeric_limits::max(); + + /// \brief 32bit unsigned integer minimum value + static const int32_t MIN_I32 = std::numeric_limits::min(); + + /// \brief 32bit unsigned integer lowest value. This is equivalent to + /// IGN_INT32_MIN, and is defined here for completeness. + static const int32_t LOW_I32 = std::numeric_limits::lowest(); + + /// \brief 32-bit unsigned integer positive infinite value + static const int32_t INF_I32 = std::numeric_limits::infinity(); + + /// \brief 64bit unsigned integer maximum value + static const uint64_t MAX_UI64 = std::numeric_limits::max(); + + /// \brief 64bit unsigned integer minimum value + static const uint64_t MIN_UI64 = std::numeric_limits::min(); + + /// \brief 64bit unsigned integer lowest value. This is equivalent to + /// IGN_UINT64_MIN, and is defined here for completeness. + static const uint64_t LOW_UI64 = std::numeric_limits::lowest(); + + /// \brief 64-bit unsigned integer positive infinite value + static const uint64_t INF_UI64 = std::numeric_limits::infinity(); + + /// \brief 64bit unsigned integer maximum value + static const int64_t MAX_I64 = std::numeric_limits::max(); + + /// \brief 64bit unsigned integer minimum value + static const int64_t MIN_I64 = std::numeric_limits::min(); + + /// \brief 64bit unsigned integer lowest value. This is equivalent to + /// IGN_INT64_MIN, and is defined here for completeness. + static const int64_t LOW_I64 = std::numeric_limits::lowest(); + + /// \brief 64-bit unsigned integer positive infinite value + static const int64_t INF_I64 = std::numeric_limits::infinity(); + + /// \brief Returns the representation of a quiet not a number (NAN) + static const int NAN_I = std::numeric_limits::quiet_NaN(); + + // variables created to deprecate macros in this file + static const double IGN_DEPRECATED(3) DPRCT_MAX_D = MAX_D; + static const double IGN_DEPRECATED(3) DPRCT_MIN_D = MIN_D; + static const double IGN_DEPRECATED(3) DPRCT_LOW_D = LOW_D; + static const double IGN_DEPRECATED(3) DPRCT_INF_D = INF_D; + static const float IGN_DEPRECATED(3) DPRCT_MAX_F = MAX_F; + static const float IGN_DEPRECATED(3) DPRCT_MIN_F = MIN_F; + static const float IGN_DEPRECATED(3) DPRCT_LOW_F = LOW_F; + static const float IGN_DEPRECATED(3) DPRCT_INF_F = INF_F; + static const uint16_t IGN_DEPRECATED(3) DPRCT_MAX_UI16 = MAX_UI16; + static const uint16_t IGN_DEPRECATED(3) DPRCT_MIN_UI16 = MIN_UI16; + static const uint16_t IGN_DEPRECATED(3) DPRCT_LOW_UI16 = LOW_UI16; + static const uint16_t IGN_DEPRECATED(3) DPRCT_INF_UI16 = INF_UI16; + static const int16_t IGN_DEPRECATED(3) DPRCT_MAX_I16 = MAX_I16; + static const int16_t IGN_DEPRECATED(3) DPRCT_MIN_I16 = MIN_I16; + static const int16_t IGN_DEPRECATED(3) DPRCT_LOW_I16 = LOW_I16; + static const int16_t IGN_DEPRECATED(3) DPRCT_INF_I16 = INF_I16; + static const uint32_t IGN_DEPRECATED(3) DPRCT_MAX_UI32 = MAX_UI32; + static const uint32_t IGN_DEPRECATED(3) DPRCT_MIN_UI32 = MIN_UI32; + static const uint32_t IGN_DEPRECATED(3) DPRCT_LOW_UI32 = LOW_UI32; + static const uint32_t IGN_DEPRECATED(3) DPRCT_INF_UI32 = INF_UI32; + static const int32_t IGN_DEPRECATED(3) DPRCT_MAX_I32 = MAX_I32; + static const int32_t IGN_DEPRECATED(3) DPRCT_MIN_I32 = MIN_I32; + static const int32_t IGN_DEPRECATED(3) DPRCT_LOW_I32 = LOW_I32; + static const int32_t IGN_DEPRECATED(3) DPRCT_INF_I32 = INF_I32; + static const uint64_t IGN_DEPRECATED(3) DPRCT_MAX_UI64 = MAX_UI64; + static const uint64_t IGN_DEPRECATED(3) DPRCT_MIN_UI64 = MIN_UI64; + static const uint64_t IGN_DEPRECATED(3) DPRCT_LOW_UI64 = LOW_UI64; + static const uint64_t IGN_DEPRECATED(3) DPRCT_INF_UI64 = INF_UI64; + static const int64_t IGN_DEPRECATED(3) DPRCT_MAX_I64 = MAX_I64; + static const int64_t IGN_DEPRECATED(3) DPRCT_MIN_I64 = MIN_I64; + static const int64_t IGN_DEPRECATED(3) DPRCT_LOW_I64 = LOW_I64; + static const int64_t IGN_DEPRECATED(3) DPRCT_INF_I64 = INF_I64; + + /// \brief Simple clamping function + /// \param[in] _v value + /// \param[in] _min minimum + /// \param[in] _max maximum + template + inline T clamp(T _v, T _min, T _max) + { + return std::max(std::min(_v, _max), _min); + } + + /// \brief check if a float is NaN + /// \param[in] _v the value + /// \return true if _v is not a number, false otherwise + inline bool isnan(float _v) + { + return (std::isnan)(_v); + } + + /// \brief check if a double is NaN + /// \param[in] _v the value + /// \return true if _v is not a number, false otherwise + inline bool isnan(double _v) + { + return (std::isnan)(_v); + } + + /// \brief Fix a nan value. + /// \param[in] _v Value to correct. + /// \return 0 if _v is NaN, _v otherwise. + inline float fixnan(float _v) + { + return isnan(_v) || std::isinf(_v) ? 0.0f : _v; + } + + /// \brief Fix a nan value. + /// \param[in] _v Value to correct. + /// \return 0 if _v is NaN, _v otherwise. + inline double fixnan(double _v) + { + return isnan(_v) || std::isinf(_v) ? 0.0 : _v; + } + + /// \brief Check if parameter is even. + /// \param[in] _v Value to check. + /// \return True if _v is even. + inline bool isEven(const int _v) + { + return !(_v % 2); + } + + /// \brief Check if parameter is even. + /// \param[in] _v Value to check. + /// \return True if _v is even. + inline bool isEven(const unsigned int _v) + { + return !(_v % 2); + } + + /// \brief Check if parameter is odd. + /// \param[in] _v Value to check. + /// \return True if _v is odd. + inline bool isOdd(const int _v) + { + return (_v % 2) != 0; + } + + /// \brief Check if parameter is odd. + /// \param[in] _v Value to check. + /// \return True if _v is odd. + inline bool isOdd(const unsigned int _v) + { + return (_v % 2) != 0; + } + + /// \brief The signum function. + /// + /// Returns 0 for zero values, -1 for negative values, + /// +1 for positive values. + /// \param[in] _value The value. + /// \return The signum of the value. + template + inline int sgn(T _value) + { + return (T(0) < _value) - (_value < T(0)); + } + + /// \brief The signum function. + /// + /// Returns 0 for zero values, -1 for negative values, + /// +1 for positive values. + /// \param[in] _value The value. + /// \return The signum of the value. + template + inline int signum(T _value) + { + return sgn(_value); + } + + /// \brief get mean of vector of values + /// \param[in] _values the vector of values + /// \return the mean + template + inline T mean(const std::vector &_values) + { + T sum = 0; + for (unsigned int i = 0; i < _values.size(); ++i) + sum += _values[i]; + return sum / _values.size(); + } + + /// \brief get variance of vector of values + /// \param[in] _values the vector of values + /// \return the squared deviation + template + inline T variance(const std::vector &_values) + { + T avg = mean(_values); + + T sum = 0; + for (unsigned int i = 0; i < _values.size(); ++i) + sum += (_values[i] - avg) * (_values[i] - avg); + return sum / _values.size(); + } + + /// \brief get the maximum value of vector of values + /// \param[in] _values the vector of values + /// \return maximum + template + inline T max(const std::vector &_values) + { + T max = std::numeric_limits::min(); + for (unsigned int i = 0; i < _values.size(); ++i) + if (_values[i] > max) + max = _values[i]; + return max; + } + + /// \brief get the minimum value of vector of values + /// \param[in] _values the vector of values + /// \return minimum + template + inline T min(const std::vector &_values) + { + T min = std::numeric_limits::max(); + for (unsigned int i = 0; i < _values.size(); ++i) + if (_values[i] < min) + min = _values[i]; + return min; + } + + /// \brief check if two values are equal, within a tolerance + /// \param[in] _a the first value + /// \param[in] _b the second value + /// \param[in] _epsilon the tolerance + template + inline bool equal(const T &_a, const T &_b, + const T &_epsilon = T(1e-6)) + { + IGN_FP_VOLATILE T diff = std::abs(_a - _b); + return diff <= _epsilon; + } + + /// \brief inequality test, within a tolerance + /// \param[in] _a the first value + /// \param[in] _b the second value + /// \param[in] _epsilon the tolerance + template + inline bool lessOrNearEqual(const T &_a, const T &_b, + const T &_epsilon = 1e-6) + { + return _a < _b + _epsilon; + } + + /// \brief inequality test, within a tolerance + /// \param[in] _a the first value + /// \param[in] _b the second value + /// \param[in] _epsilon the tolerance + template + inline bool greaterOrNearEqual(const T &_a, const T &_b, + const T &_epsilon = 1e-6) + { + return _a > _b - _epsilon; + } + + /// \brief get value at a specified precision + /// \param[in] _a the number + /// \param[in] _precision the precision + /// \return the value for the specified precision + template + inline T precision(const T &_a, const unsigned int &_precision) + { + auto p = std::pow(10, _precision); + return static_cast(std::round(_a * p) / p); + } + + /// \brief Sort two numbers, such that _a <= _b + /// \param[out] _a the first number + /// \param[out] _b the second number + template + inline void sort2(T &_a, T &_b) + { + using std::swap; + if (_b < _a) + swap(_a, _b); + } + + /// \brief Sort three numbers, such that _a <= _b <= _c + /// \param[out] _a the first number + /// \param[out] _b the second number + /// \param[out] _c the third number + template + inline void sort3(T &_a, T &_b, T &_c) + { + // _a <= _b + sort2(_a, _b); + // _a <= _c, _b <= _c + sort2(_b, _c); + // _a <= _b <= _c + 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 + inline bool isPowerOfTwo(unsigned int _x) + { + return ((_x != 0) && ((_x & (~_x + 1)) == _x)); + } + + /// \brief Get the smallest power of two that is greater or equal to + /// a given value + /// \param[in] _x the number + /// \return the same value if _x is already a power of two. Otherwise, + /// it returns the smallest power of two that is greater than _x + inline unsigned int roundUpPowerOfTwo(unsigned int _x) + { + if (_x == 0) + return 1; + + if (isPowerOfTwo(_x)) + return _x; + + while (_x & (_x - 1)) + _x = _x & (_x - 1); + + _x = _x << 1; + + return _x; + } + + /// \brief Round a number up to the nearest multiple. For example, if + /// the input number is 12 and the multiple is 10, the result is 20. + /// If the input number is negative, then the nearest multiple will be + /// greater than or equal to the input number. For example, if the input + /// number is -9 and the multiple is 2 then the output is -8. + /// \param[in] _num Input number to round up. + /// \param[in] _multiple The multiple. If the multiple is <= zero, then + /// the input number is returned. + /// \return The nearest multiple of _multiple that is greater than + /// or equal to _num. + inline int roundUpMultiple(int _num, int _multiple) + { + if (_multiple == 0) + return _num; + + int remainder = std::abs(_num) % _multiple; + if (remainder == 0) + return _num; + + if (_num < 0) + return -(std::abs(_num) - remainder); + else + return _num + _multiple - remainder; + } + + /// \brief parse string into an integer + /// \param[in] _input the string + /// \return an integer, 0 or 0 and a message in the error stream + inline int parseInt(const std::string &_input) + { + // Return NAN_I if it is empty + if (_input.empty()) + { + return NAN_I; + } + // Return 0 if it is all spaces + else if (_input.find_first_not_of(' ') == std::string::npos) + { + return 0; + } + + // Otherwise try standard library + try + { + return std::stoi(_input); + } + // if that fails, return NAN_I + catch(...) + { + return NAN_I; + } + } + + /// \brief parse string into float + /// \param _input the string + /// \return a floating point number (can be NaN) or 0 with a message in the + /// error stream + inline double parseFloat(const std::string &_input) + { + // Return NAN_D if it is empty + if (_input.empty()) + { + return NAN_D; + } + // Return 0 if it is all spaces + else if (_input.find_first_not_of(' ') == std::string::npos) + { + return 0; + } + + // Otherwise try standard library + try + { + return std::stod(_input); + } + // if that fails, return NAN_D + catch(...) + { + return NAN_D; + } + } + + /// \brief Convert a std::chrono::steady_clock::time_point to a seconds and + /// nanoseconds pair. + /// \param[in] _time The time point to convert. + /// \return A pair where the first element is the number of seconds and + /// the second is the number of nanoseconds. + inline std::pair timePointToSecNsec( + const std::chrono::steady_clock::time_point &_time) + { + auto now_ns = std::chrono::duration_cast( + _time.time_since_epoch()); + auto now_s = std::chrono::duration_cast( + _time.time_since_epoch()); + int64_t seconds = now_s.count(); + int64_t nanoseconds = std::chrono::duration_cast + (now_ns - now_s).count(); + return {seconds, nanoseconds}; + } + + /// \brief Convert seconds and nanoseconds to + /// std::chrono::steady_clock::time_point. + /// \param[in] _sec The seconds to convert. + /// \param[in] _nanosec The nanoseconds to convert. + /// \return A std::chrono::steady_clock::time_point based on the number of + /// seconds and the number of nanoseconds. + inline std::chrono::steady_clock::time_point secNsecToTimePoint( + const uint64_t &_sec, const uint64_t &_nanosec) + { + auto duration = std::chrono::seconds(_sec) + std::chrono::nanoseconds( + _nanosec); + std::chrono::steady_clock::time_point result; + using std::chrono::duration_cast; + result += duration_cast(duration); + return result; + } + + /// \brief Convert seconds and nanoseconds to + /// std::chrono::steady_clock::duration. + /// \param[in] _sec The seconds to convert. + /// \param[in] _nanosec The nanoseconds to convert. + /// \return A std::chrono::steady_clock::duration based on the number of + /// seconds and the number of nanoseconds. + inline std::chrono::steady_clock::duration secNsecToDuration( + const uint64_t &_sec, const uint64_t &_nanosec) + { + return std::chrono::seconds(_sec) + std::chrono::nanoseconds( + _nanosec); + } + + /// \brief Convert a std::chrono::steady_clock::duration to a seconds and + /// nanoseconds pair. + /// \param[in] _dur The duration to convert. + /// \return A pair where the first element is the number of seconds and + /// the second is the number of nanoseconds. + inline std::pair durationToSecNsec( + const std::chrono::steady_clock::duration &_dur) + { + auto s = std::chrono::duration_cast(_dur); + auto ns = std::chrono::duration_cast(_dur-s); + return {s.count(), ns.count()}; + } + + // TODO(anyone): Replace this with std::chrono::days. + /// This will exist in C++-20 + typedef std::chrono::duration> days; + + /// \brief break down durations + /// NOTE: the template arguments must be properly ordered according + /// to magnitude and there can be no duplicates. + /// This function uses the braces initializer to split all the templated + /// duration. The initializer will be called recursievely due the `...` + /// \param[in] d Duration to break down + /// \return A tuple based on the durations specified + template + std::tuple breakDownDurations(DurationIn d) { + std::tuple retval; + using discard = int[]; + (void)discard{0, (void(( + (std::get(retval) = + std::chrono::duration_cast(d)), + (d -= std::chrono::duration_cast( + std::get(retval))))), 0)...}; + return retval; + } + + /// \brief Convert a std::chrono::steady_clock::time_point to a string + /// \param[in] _point The std::chrono::steady_clock::time_point to convert. + /// \return A string formatted with the time_point + inline std::string timePointToString( + const std::chrono::steady_clock::time_point &_point) + { + auto duration = _point - secNsecToTimePoint(0, 0); + auto cleanDuration = breakDownDurations( + duration); + std::ostringstream output_string; + output_string << std::setw(2) << std::setfill('0') + << std::get<0>(cleanDuration).count() << " " + << std::setw(2) << std::setfill('0') + << std::get<1>(cleanDuration).count() << ":" + << std::setw(2) << std::setfill('0') + << std::get<2>(cleanDuration).count() << ":" + << std::setfill('0') << std::setw(6) + << std::fixed << std::setprecision(3) + << std::get<3>(cleanDuration).count() + + std::get<4>(cleanDuration).count()/1000.0; + return output_string.str(); + } + + /// \brief Convert a std::chrono::steady_clock::duration to a string + /// \param[in] _duration The std::chrono::steady_clock::duration to convert. + /// \return A string formatted with the duration + inline std::string durationToString( + const std::chrono::steady_clock::duration &_duration) + { + auto cleanDuration = breakDownDurations( + _duration); + std::ostringstream outputString; + outputString << std::setw(2) << std::setfill('0') + << std::get<0>(cleanDuration).count() << " " + << std::setw(2) << std::setfill('0') + << std::get<1>(cleanDuration).count() << ":" + << std::setw(2) << std::setfill('0') + << std::get<2>(cleanDuration).count() << ":" + << std::setfill('0') << std::setw(6) + << std::fixed << std::setprecision(3) + << std::get<3>(cleanDuration).count() + + std::get<4>(cleanDuration).count()/1000.0; + return outputString.str(); + } + + // The following regex takes a time string in the general format of + // "dd hh:mm:ss.nnn" where n is milliseconds, if just one number is + // provided, it is assumed to be seconds + static const std::regex time_regex( + "^([0-9]+ ){0,1}" // day: + // Any positive integer + + "(?:([1-9]:|[0-1][0-9]:|2[0-3]:){0,1}" // hour: + // 1 - 9: + // 01 - 19: + // 20 - 23: + + "([0-9]:|[0-5][0-9]:)){0,1}" // minute: + // 0 - 9: + // 00 - 59: + + "(?:([0-9]|[0-5][0-9]){0,1}" // second: + // 0 - 9 + // 00 - 59 + + "(\\.[0-9]{1,3}){0,1})$"); // millisecond: + // .0 - .9 + // .00 - .99 + // .000 - 0.999 + + + /// \brief Check if the given string represents a time. + /// An example time string is "0 00:00:00.000", which has the format + /// "DAYS HOURS:MINUTES:SECONDS.MILLISECONDS" + /// \return True if the regex was able to split the string otherwise False + inline bool isTimeString(const std::string &_timeString) + { + std::smatch matches; + + // `matches` should always be a size of 6 as there are 6 matching + // groups in the regex. + // 1. The whole regex + // 2. The days + // 3. The hours + // 4. The minutes + // 5. The seconds + // 6. The milliseconds + // Note that the space will remain in the day match, the colon + // will remain in the hour and minute matches, and the period will + // remain in the millisecond match + if (!std::regex_search(_timeString, matches, time_regex) || + matches.size() != 6) + return false; + + std::string dayString = matches[1]; + + // Days are the only unbounded number, so check first to see if stoi + // runs successfully + if (!dayString.empty()) + { + // Erase the space + dayString.erase(dayString.length() - 1); + try + { + std::stoi(dayString); + } + catch (const std::out_of_range &) + { + return false; + } + } + + return true; + } + + /// \brief Split a std::chrono::steady_clock::duration to a string + /// \param[in] _timeString The string to convert in general format + /// \param[out] numberDays number of days in the string + /// \param[out] numberHours number of hours in the string + /// \param[out] numberMinutes number of minutes in the string + /// \param[out] numberSeconds number of seconds in the string + /// \param[out] numberMilliseconds number of milliseconds in the string + /// \return True if the regex was able to split the string otherwise False + inline bool splitTimeBasedOnTimeRegex( + const std::string &_timeString, + uint64_t & numberDays, uint64_t & numberHours, + uint64_t & numberMinutes, uint64_t & numberSeconds, + uint64_t & numberMilliseconds) + { + std::smatch matches; + + // `matches` should always be a size of 6 as there are 6 matching + // groups in the regex. + // 1. The whole regex + // 2. The days + // 3. The hours + // 4. The minutes + // 5. The seconds + // 6. The milliseconds + // We can also index them as such below. + // Note that the space will remain in the day match, the colon + // will remain in the hour and minute matches, and the period will + // remain in the millisecond match + if (!std::regex_search(_timeString, matches, time_regex) || + matches.size() != 6) + return false; + + std::string dayString = matches[1]; + std::string hourString = matches[2]; + std::string minuteString = matches[3]; + std::string secondString = matches[4]; + std::string millisecondString = matches[5]; + + // Days are the only unbounded number, so check first to see if stoi + // runs successfully + if (!dayString.empty()) + { + // Erase the space + dayString.erase(dayString.length() - 1); + try + { + numberDays = std::stoi(dayString); + } + catch (const std::out_of_range &) + { + return false; + } + } + + if (!hourString.empty()) + { + // Erase the colon + hourString.erase(hourString.length() - 1); + numberHours = std::stoi(hourString); + } + + if (!minuteString.empty()) + { + // Erase the colon + minuteString.erase(minuteString.length() - 1); + numberMinutes = std::stoi(minuteString); + } + + if (!secondString.empty()) + { + numberSeconds = std::stoi(secondString); + } + + if (!millisecondString.empty()) + { + // Erase the period + millisecondString.erase(0, 1); + + // Multiplier because "4" = 400 ms, "04" = 40 ms, and "004" = 4 ms + numberMilliseconds = std::stoi(millisecondString) * + static_cast(1000 / pow(10, millisecondString.length())); + } + return true; + } + + /// \brief Convert a string to a std::chrono::steady_clock::duration + /// \param[in] _timeString The string to convert in general format + /// "dd hh:mm:ss.nnn" where n is millisecond value + /// \return A std::chrono::steady_clock::duration containing the + /// string's time value. If it isn't possible to convert, the duration will + /// be zero. + inline std::chrono::steady_clock::duration stringToDuration( + const std::string &_timeString) + { + using namespace std::chrono_literals; + std::chrono::steady_clock::duration duration{ + std::chrono::steady_clock::duration::zero()}; + + if (_timeString.empty()) + return duration; + + uint64_t numberDays = 0; + uint64_t numberHours = 0; + uint64_t numberMinutes = 0; + uint64_t numberSeconds = 0; + uint64_t numberMilliseconds = 0; + + if (!splitTimeBasedOnTimeRegex(_timeString, numberDays, numberHours, + numberMinutes, numberSeconds, + numberMilliseconds)) + { + return duration; + } + + // TODO(anyone): Replace below day conversion with std::chrono::days. + /// This will exist in C++-20 + duration = std::chrono::steady_clock::duration::zero(); + auto delta = std::chrono::milliseconds(numberMilliseconds) + + std::chrono::seconds(numberSeconds) + + std::chrono::minutes(numberMinutes) + + std::chrono::hours(numberHours) + + std::chrono::hours(24 * numberDays); + duration += delta; + + return duration; + } + + /// \brief Convert a string to a std::chrono::steady_clock::time_point + /// \param[in] _timeString The string to convert in general format + /// "dd hh:mm:ss.nnn" where n is millisecond value + /// \return A std::chrono::steady_clock::time_point containing the + /// string's time value. If it isn't possible to convert, the time will + /// be negative 1 second. + inline std::chrono::steady_clock::time_point stringToTimePoint( + const std::string &_timeString) + { + using namespace std::chrono_literals; + std::chrono::steady_clock::time_point timePoint{-1s}; + + if (_timeString.empty()) + return timePoint; + + uint64_t numberDays = 0; + uint64_t numberHours = 0; + uint64_t numberMinutes = 0; + uint64_t numberSeconds = 0; + uint64_t numberMilliseconds = 0; + + if (!splitTimeBasedOnTimeRegex(_timeString, numberDays, numberHours, + numberMinutes, numberSeconds, + numberMilliseconds)) + { + return timePoint; + } + + // TODO(anyone): Replace below day conversion with std::chrono::days. + /// This will exist in C++-20 + timePoint = math::secNsecToTimePoint(0, 0); + auto duration = std::chrono::milliseconds(numberMilliseconds) + + std::chrono::seconds(numberSeconds) + + std::chrono::minutes(numberMinutes) + + std::chrono::hours(numberHours) + + std::chrono::hours(24 * numberDays); + timePoint += duration; + + return timePoint; + } + + // Degrade precision on Windows, which cannot handle 'long double' + // values properly. See the implementation of Unpair. + // 32 bit ARM processors also define 'long double' to be the same + // size as 'double', and must also be degraded +#if defined _MSC_VER || defined __arm__ + using PairInput = uint16_t; + using PairOutput = uint32_t; +#else + using PairInput = uint32_t; + using PairOutput = uint64_t; +#endif + + /// \brief A pairing function that maps two values to a unique third + /// value. This is an implement of Szudzik's function. + /// \param[in] _a First value, must be a non-negative integer. On + /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t. + /// \param[in] _b Second value, must be a non-negative integer. On + /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t. + /// \return A unique non-negative integer value. On Windows the return + /// value is uint32_t. On Linux/OSX the return value is uint64_t + /// \sa Unpair + PairOutput IGNITION_MATH_VISIBLE Pair( + const PairInput _a, const PairInput _b); + + /// \brief The reverse of the Pair function. Accepts a key, produced + /// from the Pair function, and returns a tuple consisting of the two + /// non-negative integer values used to create the _key. + /// \param[in] _key A non-negative integer generated from the Pair + /// function. On Windows this value is uint32_t. On Linux/OSX, this + /// value is uint64_t. + /// \return A tuple that consists of the two non-negative integers that + /// will generate _key when used with the Pair function. On Windows the + /// tuple contains two uint16_t values. On Linux/OSX the tuple contains + /// two uint32_t values. + /// \sa Pair + std::tuple IGNITION_MATH_VISIBLE Unpair( + const PairOutput _key); + } + } +} + +#endif diff --git a/include/gz/math/Inertial.hh b/include/gz/math/Inertial.hh new file mode 100644 index 000000000..8a172af00 --- /dev/null +++ b/include/gz/math/Inertial.hh @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2016 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_INERTIAL_HH_ +#define GZ_MATH_INERTIAL_HH_ + +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Pose3.hh" + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Inertial Inertial.hh ignition/math/Inertial.hh + /// \brief The Inertial object provides a representation for the mass and + /// inertia matrix of a body B. The components of the inertia matrix are + /// expressed in what we call the "inertial" frame Bi of the body, i.e. + /// the frame in which these inertia components are measured. The inertial + /// frame Bi must be located at the center of mass of the body, but not + /// necessarily aligned with the body’s frame. In addition, this class + /// allows users to specify a frame F for these inertial properties by + /// specifying the pose X_FBi of the inertial frame Bi in the + /// inertial object frame F. + /// + /// For information about the X_FBi notation, see + /// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html + template + class Inertial + { + /// \brief Default Constructor + public: Inertial() + {} + + /// \brief Constructs an inertial object from the mass matrix for a body + /// B, about its center of mass Bcm, and expressed in a frame that we’ll + /// call the "inertial" frame Bi, i.e. the frame in which the components + /// of the mass matrix are specified (see this class’s documentation for + /// details). The pose object specifies the pose X_FBi of the inertial + /// frame Bi in the frame F of this inertial object + /// (see class’s documentation). + /// \param[in] _massMatrix Mass and inertia matrix. + /// \param[in] _pose Pose of center of mass reference frame. + public: Inertial(const MassMatrix3 &_massMatrix, + const Pose3 &_pose) + : massMatrix(_massMatrix), pose(_pose) + {} + + /// \brief Copy constructor. + /// \param[in] _inertial Inertial element to copy + public: Inertial(const Inertial &_inertial) + : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose()) + {} + + /// \brief Destructor. + public: virtual ~Inertial() {} + + /// \brief Set the mass and inertia matrix. + /// + /// \param[in] _m New MassMatrix3 object. + /// \param[in] _tolerance Tolerance is passed to + /// MassMatrix3::IsValid and is the amount of error + /// to accept when checking whether the MassMatrix3 _m is valid. + /// Refer to MassMatrix3::Epsilon for detailed description of + /// _tolerance. + /// + /// \return True if the MassMatrix3 is valid. + public: bool SetMassMatrix(const MassMatrix3 &_m, + const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE) + { + this->massMatrix = _m; + return this->massMatrix.IsValid(_tolerance); + } + + /// \brief Get the mass and inertia matrix. + /// \return The mass matrix about the body’s center of mass and + /// expressed in the inertial frame Bi as defined by this class’s + /// documentation + public: const MassMatrix3 &MassMatrix() const + { + return this->massMatrix; + } + + /// \brief Set the pose of the center of mass reference frame. + /// \param[in] _pose New pose. + /// \return True if the MassMatrix3 is valid. + public: bool SetPose(const Pose3 &_pose) + { + this->pose = _pose; + return this->massMatrix.IsValid(); + } + + /// \brief Get the pose of the center of mass reference frame. + /// \return The pose of the inertial frame Bi in the frame F of this + /// Inertial object as defined by this class’s documentation. + public: const Pose3 &Pose() const + { + return this->pose; + } + + /// \copydoc Moi() const + /// \deprecated See Matrix3 Moi() const + public: Matrix3 IGN_DEPRECATED(5.0) MOI() const + { + return this->Moi(); + } + + /// \brief Get the moment of inertia matrix computer about the body's + /// center of mass and expressed in this Inertial object’s frame F. + /// \return The inertia matrix computed about the body’s center of + /// mass and expressed in this Inertial object’s frame F, as defined + /// in this class’s documentation. + public: Matrix3 Moi() const + { + auto R = Matrix3(this->pose.Rot()); + return R * this->massMatrix.Moi() * R.Transposed(); + } + + /// \brief Set the inertial pose rotation without affecting the + /// MOI in the base coordinate frame. + /// \param[in] _q New rotation for inertial pose. + /// \return True if the MassMatrix3 is valid. + public: bool SetInertialRotation(const Quaternion &_q) + { + auto moi = this->Moi(); + this->pose.Rot() = _q; + auto R = Matrix3(_q); + return this->massMatrix.SetMoi(R.Transposed() * moi * R); + } + + /// \brief Set the MassMatrix rotation (eigenvectors of inertia matrix) + /// without affecting the MOI in the base coordinate frame. + /// Note that symmetries in inertia matrix may prevent the output of + /// MassMatrix3::PrincipalAxesOffset to match this function's input _q, + /// but it is guaranteed that the MOI in the base frame will not change. + /// A negative value of _tol (such as -1e-6) can be passed to ensure + /// that diagonal values are always sorted. + /// \param[in] _q New rotation. + /// \param[in] _tol Relative tolerance given by absolute value + /// of _tol. This is passed to the MassMatrix3 + /// PrincipalMoments and PrincipalAxesOffset functions. + /// \return True if the MassMatrix3 is valid. + public: bool SetMassMatrixRotation(const Quaternion &_q, + const T _tol = 1e-6) + { + this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) * + _q.Inverse(); + const auto moments = this->MassMatrix().PrincipalMoments(_tol); + const auto diag = Matrix3( + moments[0], 0, 0, + 0, moments[1], 0, + 0, 0, moments[2]); + const auto R = Matrix3(_q); + return this->massMatrix.SetMoi(R * diag * R.Transposed()); + } + + /// \brief Equal operator. + /// \param[in] _inertial Inertial to copy. + /// \return Reference to this object. + public: Inertial &operator=(const Inertial &_inertial) + { + this->massMatrix = _inertial.MassMatrix(); + this->pose = _inertial.Pose(); + + return *this; + } + + /// \brief Equality comparison operator. + /// \param[in] _inertial Inertial to copy. + /// \return true if each component is equal within a default tolerance, + /// false otherwise + public: bool operator==(const Inertial &_inertial) const + { + return (this->pose == _inertial.Pose()) && + (this->massMatrix == _inertial.MassMatrix()); + } + + /// \brief Inequality test operator + /// \param[in] _inertial Inertial to test + /// \return True if not equal (using the default tolerance of 1e-6) + public: bool operator!=(const Inertial &_inertial) const + { + return !(*this == _inertial); + } + + /// \brief Adds inertial properties to current object. + /// The mass, center of mass location, and inertia matrix are updated + /// as long as the total mass is positive. + /// \param[in] _inertial Inertial to add. + /// \return Reference to this object. + public: Inertial &operator+=(const Inertial &_inertial) + { + T m1 = this->massMatrix.Mass(); + T m2 = _inertial.MassMatrix().Mass(); + + // Total mass + T mass = m1 + m2; + + // Only continue if total mass is positive + if (mass <= 0) + { + return *this; + } + + auto com1 = this->Pose().Pos(); + auto com2 = _inertial.Pose().Pos(); + // New center of mass location in base frame + auto com = (m1*com1 + m2*com2) / mass; + + // Components of new moment of inertia matrix + Vector3 ixxyyzz; + Vector3 ixyxzyz; + // First add matrices in base frame + { + auto moi = this->Moi() + _inertial.Moi(); + ixxyyzz = Vector3(moi(0, 0), moi(1, 1), moi(2, 2)); + ixyxzyz = Vector3(moi(0, 1), moi(0, 2), moi(1, 2)); + } + // Then account for parallel axis theorem + { + auto dc = com1 - com; + ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2)); + ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2)); + ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2)); + ixyxzyz.X() -= m1 * dc[0] * dc[1]; + ixyxzyz.Y() -= m1 * dc[0] * dc[2]; + ixyxzyz.Z() -= m1 * dc[1] * dc[2]; + } + { + auto dc = com2 - com; + ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2)); + ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2)); + ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2)); + ixyxzyz.X() -= m2 * dc[0] * dc[1]; + ixyxzyz.Y() -= m2 * dc[0] * dc[2]; + ixyxzyz.Z() -= m2 * dc[1] * dc[2]; + } + this->massMatrix = MassMatrix3(mass, ixxyyzz, ixyxzyz); + this->pose = Pose3(com, Quaternion::Identity); + + return *this; + } + + /// \brief Adds inertial properties to current object. + /// The mass, center of mass location, and inertia matrix are updated + /// as long as the total mass is positive. + /// \param[in] _inertial Inertial to add. + /// \return Sum of inertials as new object. + public: const Inertial operator+(const Inertial &_inertial) const + { + return Inertial(*this) += _inertial; + } + + /// \brief Mass and inertia matrix of the object expressed in the + /// center of mass reference frame. + private: MassMatrix3 massMatrix; + + /// \brief Pose offset of center of mass reference frame relative + /// to a base frame. + private: Pose3 pose; + }; + + typedef Inertial Inertiald; + typedef Inertial Inertialf; + } + } +} +#endif diff --git a/include/gz/math/Interval.hh b/include/gz/math/Interval.hh new file mode 100644 index 000000000..523a63b66 --- /dev/null +++ b/include/gz/math/Interval.hh @@ -0,0 +1,299 @@ +/* + * 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_INTERVAL_HH_ +#define GZ_MATH_INTERVAL_HH_ + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Interval Interval.hh ignition/math/Interval.hh + /// \brief The Interval class represents a range of real numbers. + /// Intervals may be open (a, b), left-closed [a, b), right-closed + /// (a, b], or fully closed [a, b]. + /// + /// ## Example + /// + /// \snippet examples/interval_example.cc complete + template + class Interval + { + /// \brief An unbounded interval (-∞, ∞) + public: static const Interval &Unbounded; + + /// \brief Constructor + public: Interval() = default; + + /// \brief Constructor + /// \param[in] _leftValue leftmost interval value + /// \param[in] _leftClosed whether the interval is + /// left-closed or not + /// \param[in] _rightValue rightmost interval value + /// \param[in] _rightClosed whether the interval + /// is right-closed or not + public: constexpr Interval( + T _leftValue, bool _leftClosed, + T _rightValue, bool _rightClosed) + : leftValue(std::move(_leftValue)), + rightValue(std::move(_rightValue)), + leftClosed(_leftClosed), + rightClosed(_rightClosed) + { + } + + /// \brief Make an open interval (`_leftValue`, `_rightValue`) + /// \param[in] _leftValue leftmost interval value + /// \param[in] _rightValue rightmost interval value + /// \return the open interval + public: static constexpr Interval + Open(T _leftValue, T _rightValue) + { + return Interval( + std::move(_leftValue), false, + std::move(_rightValue), false); + } + + /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`) + /// \param[in] _leftValue leftmost interval value + /// \param[in] _rightValue rightmost interval value + /// \return the left-closed interval + public: static constexpr Interval + LeftClosed(T _leftValue, T _rightValue) + { + return Interval( + std::move(_leftValue), true, + std::move(_rightValue), false); + } + + /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`] + /// \param[in] _leftValue leftmost interval value + /// \param[in] _rightValue rightmost interval value + /// \return the left-closed interval + public: static constexpr Interval + RightClosed(T _leftValue, T _rightValue) + { + return Interval( + std::move(_leftValue), false, + std::move(_rightValue), true); + } + + /// \brief Make a closed interval [`_leftValue`, `_rightValue`] + /// \param[in] _leftValue leftmost interval value + /// \param[in] _rightValue rightmost interval value + /// \return the closed interval + public: static constexpr Interval + Closed(T _leftValue, T _rightValue) + { + return Interval{ + std::move(_leftValue), true, + std::move(_rightValue), true}; + } + + /// \brief Get the leftmost interval value + /// \return the leftmost interval value + public: const T &LeftValue() const { return this->leftValue; } + + /// \brief Check if the interval is left-closed + /// \return true if the interval is left-closed, false otherwise + public: bool IsLeftClosed() const { return this->leftClosed; } + + /// \brief Get the rightmost interval value + /// \return the rightmost interval value + public: const T &RightValue() const { return this->rightValue; } + + /// \brief Check if the interval is right-closed + /// \return true if the interval is right-closed, false otherwise + public: bool IsRightClosed() const { return this->rightClosed; } + + /// \brief Check if the interval is empty + /// Some examples of empty intervals include + /// (a, a), [a, a), and [a + 1, a]. + /// \return true if it is empty, false otherwise + public: bool Empty() const + { + if (this->leftClosed && this->rightClosed) + { + return this->rightValue < this->leftValue; + } + return this->rightValue <= this->leftValue; + } + + /// \brief Check if the interval contains `_value` + /// \param[in] _value value to check for membership + /// \return true if it is contained, false otherwise + public: bool Contains(const T &_value) const + { + if (this->leftClosed && this->rightClosed) + { + return this->leftValue <= _value && _value <= this->rightValue; + } + if (this->leftClosed) + { + return this->leftValue <= _value && _value < this->rightValue; + } + if (this->rightClosed) + { + return this->leftValue < _value && _value <= this->rightValue; + } + return this->leftValue < _value && _value < this->rightValue; + } + + /// \brief Check if the interval contains `_other` interval + /// \param[in] _other interval to check for membership + /// \return true if it is contained, false otherwise + public: bool Contains(const Interval &_other) const + { + if (this->Empty() || _other.Empty()) + { + return false; + } + if (!this->leftClosed && _other.leftClosed) + { + if (_other.leftValue <= this->leftValue) + { + return false; + } + } + else + { + if (_other.leftValue < this->leftValue) + { + return false; + } + } + if (!this->rightClosed && _other.rightClosed) + { + if (this->rightValue <= _other.rightValue) + { + return false; + } + } + else + { + if (this->rightValue < _other.rightValue) + { + return false; + } + } + return true; + } + + /// \brief Check if the interval intersects `_other` interval + /// \param[in] _other interval to check for intersection + /// \return true if both intervals intersect, false otherwise + public: bool Intersects(const Interval &_other) const + { + if (this->Empty() || _other.Empty()) + { + return false; + } + if (this->rightClosed && _other.leftClosed) + { + if (this->rightValue < _other.leftValue) + { + return false; + } + } + else + { + if (this->rightValue <= _other.leftValue) + { + return false; + } + } + if (_other.rightClosed && this->leftClosed) + { + if (_other.rightValue < this->leftValue) + { + return false; + } + } + else + { + if (_other.rightValue <= this->leftValue) + { + return false; + } + } + return true; + } + + /// \brief Equality test operator + /// \param _other interval to check for equality + /// \return true if intervals are equal, false otherwise + public: bool operator==(const Interval &_other) const + { + return this->Contains(_other) && _other.Contains(*this); + } + + /// \brief Inequality test operator + /// \param _other interval to check for inequality + /// \return true if intervals are unequal, false otherwise + public: bool operator!=(const Interval &_other) const + { + return !this->Contains(_other) || !_other.Contains(*this); + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _interval Interval to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Interval &_interval) + { + return _out << (_interval.leftClosed ? "[" : "(") + << _interval.leftValue << ", " << _interval.rightValue + << (_interval.rightClosed ? "]" : ")"); + } + + /// \brief The leftmost interval value + private: T leftValue{0}; + /// \brief The righmost interval value + private: T rightValue{0}; + /// \brief Whether the interval is left-closed or not + private: bool leftClosed{false}; + /// \brief Whether the interval is right-closed or not + private: bool rightClosed{false}; + }; + + namespace detail { + template + constexpr Interval gUnboundedInterval = + Interval::Open(-std::numeric_limits::infinity(), + std::numeric_limits::infinity()); + } // namespace detail + template + const Interval &Interval::Unbounded = detail::gUnboundedInterval; + + using Intervalf = Interval; + using Intervald = Interval; + } + } +} + +#endif diff --git a/include/gz/math/Kmeans.hh b/include/gz/math/Kmeans.hh new file mode 100644 index 000000000..cd6e714ce --- /dev/null +++ b/include/gz/math/Kmeans.hh @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 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_KMEANS_HH_ +#define GZ_MATH_KMEANS_HH_ + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private data + class KmeansPrivate; + + /// \class Kmeans Kmeans.hh math/gzmath.hh + /// \brief K-Means clustering algorithm. Given a set of observations, + /// k-means partitions the observations into k sets so as to minimize the + /// within-cluster sum of squares. + /// Description based on http://en.wikipedia.org/wiki/K-means_clustering. + class IGNITION_MATH_VISIBLE Kmeans + { + /// \brief constructor + /// \param[in] _obs Set of observations to cluster. + public: explicit Kmeans(const std::vector &_obs); + + /// \brief Destructor. + public: virtual ~Kmeans(); + + /// \brief Get the observations to cluster. + /// \return The vector of observations. + public: std::vector Observations() const; + + /// \brief Set the observations to cluster. + /// \param[in] _obs The new vector of observations. + /// \return True if the vector is not empty or false otherwise. + public: bool Observations(const std::vector &_obs); + + /// \brief Add observations to the cluster. + /// \param[in] _obs Vector of observations. + /// \return True if the _obs vector is not empty or false otherwise. + public: bool AppendObservations(const std::vector &_obs); + + /// \brief Executes the k-means algorithm. + /// \param[in] _k Number of partitions to cluster. + /// \param[out] _centroids Vector of centroids. Each element contains the + /// centroid of one cluster. + /// \param[out] _labels Vector of labels. The size of this vector is + /// equals to the number of observations. Each element represents the + /// cluster to which observation belongs. + /// \return True when the operation succeed or false otherwise. The + /// operation will fail if the number of observations is not positive, + /// if the number of clusters is non positive, or if the number of + /// clusters if greater than the number of observations. + public: bool Cluster(int _k, + std::vector &_centroids, + std::vector &_labels); + + /// \brief Given an observation, it returns the closest centroid to it. + /// \param[in] _p Point to check. + /// \return The index of the closest centroid to the point _p. + private: unsigned int ClosestCentroid(const Vector3d &_p) const; + + /// \brief Private data pointer + private: KmeansPrivate *dataPtr; + }; + } + } +} + +#endif diff --git a/include/gz/math/Line2.hh b/include/gz/math/Line2.hh new file mode 100644 index 000000000..3c40b0848 --- /dev/null +++ b/include/gz/math/Line2.hh @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2014 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_LINE2_HH_ +#define GZ_MATH_LINE2_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Line2 Line2.hh ignition/math/Line2.hh + /// \brief A two dimensional line segment. The line is defined by a + /// start and end point. + template + class Line2 + { + /// \brief Constructor. + /// \param[in] _ptA Start point of the line segment + /// \param[in] _ptB End point of the line segment + public: Line2(const math::Vector2 &_ptA, const math::Vector2 &_ptB) + { + this->Set(_ptA, _ptB); + } + + /// \brief Constructor. + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + public: Line2(double _x1, double _y1, double _x2, double _y2) + { + this->Set(_x1, _y1, _x2, _y2); + } + + /// \brief Set the start and end point of the line segment + /// \param[in] _ptA Start point of the line segment + /// \param[in] _ptB End point of the line segment + public: void Set(const math::Vector2 &_ptA, + const math::Vector2 &_ptB) + { + this->pts[0] = _ptA; + this->pts[1] = _ptB; + } + + /// \brief Set the start and end point of the line segment + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + public: void Set(double _x1, double _y1, double _x2, double _y2) + { + this->pts[0].Set(_x1, _y1); + this->pts[1].Set(_x2, _y2); + } + + /// \brief Return the cross product of this line and the given line. + /// Give 'a' as this line and 'b' as given line, the equation is: + /// (a.start.x - a.end.x) * (b.start.y - b.end.y) - + /// (a.start.y - a.end.y) * (b.start.x - b.end.x) + /// \param[in] _line Line for the cross product computation. + /// \return Return the cross product of this line and the given line. + public: double CrossProduct(const Line2 &_line) const + { + return (this->pts[0].X() - this->pts[1].X()) * + (_line[0].Y() -_line[1].Y()) - + (this->pts[0].Y() - this->pts[1].Y()) * + (_line[0].X() - _line[1].X()); + } + + /// \brief Return the cross product of this line and the given point. + /// Given 'a' and 'b' as the start and end points, the equation is: + // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y) + /// \param[in] _pt Point for the cross product computation. + /// \return Return the cross product of this line and the given point. + public: double CrossProduct(const Vector2 &_pt) const + { + return (_pt.Y() - this->pts[0].Y()) * + (this->pts[1].X() - this->pts[0].X()) - + (_pt.X() - this->pts[0].X()) * + (this->pts[1].Y() - this->pts[0].Y()); + } + + /// \brief Check if the given point is collinear with this line. + /// \param[in] _pt The point to check. + /// \param[in] _epsilon The error bounds within which the collinear + /// check will return true. + /// \return Return true if the point is collinear with this line, false + /// otherwise. + public: bool Collinear(const math::Vector2 &_pt, + double _epsilon = 1e-6) const + { + return math::equal(this->CrossProduct(_pt), + 0., _epsilon); + } + + /// \brief Check if the given line is parallel with this line. + /// \param[in] _line The line to check. + /// \param[in] _epsilon The error bounds within which the parallel + /// check will return true. + /// \return Return true if the line is parallel with this line, false + /// otherwise. Return true if either line is a point (line with zero + /// length). + public: bool Parallel(const math::Line2 &_line, + double _epsilon = 1e-6) const + { + return math::equal(this->CrossProduct(_line), + 0., _epsilon); + } + + /// \brief Check if the given line is collinear with this line. This + /// is the AND of Parallel and Intersect. + /// \param[in] _line The line to check. + /// \param[in] _epsilon The error bounds within which the collinear + /// check will return true. + /// \return Return true if the line is collinear with this line, false + /// otherwise. + public: bool Collinear(const math::Line2 &_line, + double _epsilon = 1e-6) const + { + return this->Parallel(_line, _epsilon) && + this->Intersect(_line, _epsilon); + } + + /// \brief Return whether the given point is on this line segment. + /// \param[in] _pt Point to check. + /// \param[in] _epsilon The error bounds within which the OnSegment + /// check will return true. + /// \return True if the point is on the segement. + public: bool OnSegment(const math::Vector2 &_pt, + double _epsilon = 1e-6) const + { + return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon); + } + + /// \brief Check if the given point is between the start and end + /// points of the line segment. This does not imply that the point is + /// on the segment. + /// \param[in] _pt Point to check. + /// \param[in] _epsilon The error bounds within which the within + /// check will return true. + /// \return True if the point is on the segement. + public: bool Within(const math::Vector2 &_pt, + double _epsilon = 1e-6) const + { + return _pt.X() <= std::max(this->pts[0].X(), + this->pts[1].X()) + _epsilon && + _pt.X() >= std::min(this->pts[0].X(), + this->pts[1].X()) - _epsilon && + _pt.Y() <= std::max(this->pts[0].Y(), + this->pts[1].Y()) + _epsilon && + _pt.Y() >= std::min(this->pts[0].Y(), + this->pts[1].Y()) - _epsilon; + } + + /// \brief Check if this line intersects the given line segment. + /// \param[in] _line The line to check for intersection. + /// \param[in] _epsilon The error bounds within which the intersection + /// check will return true. + /// \return True if an intersection was found. + public: bool Intersect(const Line2 &_line, + double _epsilon = 1e-6) const + { + static math::Vector2 ignore; + return this->Intersect(_line, ignore, _epsilon); + } + + /// \brief Check if this line intersects the given line segment. The + /// point of intersection is returned in the _result parameter. + /// \param[in] _line The line to check for intersection. + /// \param[out] _pt The point of intersection. This value is only + /// valid if the return value is true. + /// \param[in] _epsilon The error bounds within which the intersection + /// check will return true. + /// \return True if an intersection was found. + public: bool Intersect(const Line2 &_line, math::Vector2 &_pt, + double _epsilon = 1e-6) const + { + double d = this->CrossProduct(_line); + + // d is zero if the two line are collinear. Must check special + // cases. + if (math::equal(d, 0.0, _epsilon)) + { + // Check if _line's starting point is on the line. + if (this->Within(_line[0], _epsilon)) + { + _pt = _line[0]; + return true; + } + // Check if _line's ending point is on the line. + else if (this->Within(_line[1], _epsilon)) + { + _pt = _line[1]; + return true; + } + // Other wise return false. + else + return false; + } + + _pt.X((_line[0].X() - _line[1].X()) * + (this->pts[0].X() * this->pts[1].Y() - + this->pts[0].Y() * this->pts[1].X()) - + (this->pts[0].X() - this->pts[1].X()) * + (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X())); + + _pt.Y((_line[0].Y() - _line[1].Y()) * + (this->pts[0].X() * this->pts[1].Y() - + this->pts[0].Y() * this->pts[1].X()) - + (this->pts[0].Y() - this->pts[1].Y()) * + (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X())); + + _pt /= d; + + if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) || + _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) || + _pt.X() < std::min(_line[0].X(), _line[1].X()) || + _pt.X() > std::max(_line[0].X(), _line[1].X())) + { + return false; + } + + if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) || + _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) || + _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) || + _pt.Y() > std::max(_line[0].Y(), _line[1].Y())) + { + return false; + } + + return true; + } + + /// \brief Get the length of the line + /// \return The length of the line. + public: T Length() const + { + return sqrt((this->pts[0].X() - this->pts[1].X()) * + (this->pts[0].X() - this->pts[1].X()) + + (this->pts[0].Y() - this->pts[1].Y()) * + (this->pts[0].Y() - this->pts[1].Y())); + } + + /// \brief Get the slope of the line + /// \return The slope of the line, NAN_D if the line is vertical. + public: double Slope() const + { + if (math::equal(this->pts[1].X(), this->pts[0].X())) + return NAN_D; + + return (this->pts[1].Y() - this->pts[0].Y()) / + static_cast(this->pts[1].X() - this->pts[0].X()); + } + + /// \brief Equality operator. + /// \param[in] _line Line to compare for equality. + /// \return True if the given line is equal to this line + public: bool operator==(const Line2 &_line) const + { + return this->pts[0] == _line[0] && this->pts[1] == _line[1]; + } + + /// \brief Inequality operator. + /// \param[in] _line Line to compare for inequality. + /// \return True if the given line is not to this line + public: bool operator!=(const Line2 &_line) const + { + return !(*this == _line); + } + + /// \brief Get the start or end point. + /// \param[in] _index 0 = start point, 1 = end point. The _index + /// is clamped to the range [0, 1] + public: math::Vector2 operator[](size_t _index) const + { + return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; + } + + /// \brief Stream extraction operator + /// \param[in] _out output stream + /// \param[in] _line Line2 to output + /// \return The stream + public: friend std::ostream &operator<<( + std::ostream &_out, const Line2 &_line) + { + _out << _line[0] << " " << _line[1]; + return _out; + } + + private: math::Vector2 pts[2]; + }; + + + typedef Line2 Line2i; + typedef Line2 Line2d; + typedef Line2 Line2f; + } + } +} +#endif diff --git a/include/gz/math/Line3.hh b/include/gz/math/Line3.hh new file mode 100644 index 000000000..eeac8f15c --- /dev/null +++ b/include/gz/math/Line3.hh @@ -0,0 +1,424 @@ +/* + * Copyright (C) 2015 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_LINE3_HH_ +#define GZ_MATH_LINE3_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Line3 Line3.hh ignition/math/Line3.hh + /// \brief A three dimensional line segment. The line is defined by a + /// start and end point. + template + class Line3 + { + /// \brief Line Constructor + public: Line3() = default; + + /// \brief Copy constructor + /// \param[in] _line a line object + public: Line3(const Line3 &_line) + { + this->pts[0] = _line[0]; + this->pts[1] = _line[1]; + } + + /// \brief Constructor. + /// \param[in] _ptA Start point of the line segment + /// \param[in] _ptB End point of the line segment + public: Line3(const math::Vector3 &_ptA, const math::Vector3 &_ptB) + { + this->Set(_ptA, _ptB); + } + + /// \brief 2D Constructor where Z coordinates are 0 + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + public: Line3(const double _x1, const double _y1, + const double _x2, const double _y2) + { + this->Set(_x1, _y1, _x2, _y2); + } + + /// \brief Constructor. + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _z1 Z coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + /// \param[in] _z2 Z coordinate of the end point. + public: Line3(const double _x1, const double _y1, + const double _z1, const double _x2, + const double _y2, const double _z2) + { + this->Set(_x1, _y1, _z1, _x2, _y2, _z2); + } + + /// \brief Set the start and end point of the line segment + /// \param[in] _ptA Start point of the line segment + /// \param[in] _ptB End point of the line segment + public: void Set(const math::Vector3 &_ptA, + const math::Vector3 &_ptB) + { + this->pts[0] = _ptA; + this->pts[1] = _ptB; + } + + /// \brief Set the start point of the line segment + /// \param[in] _ptA Start point of the line segment + public: void SetA(const math::Vector3 &_ptA) + { + this->pts[0] = _ptA; + } + + /// \brief Set the end point of the line segment + /// \param[in] _ptB End point of the line segment + public: void SetB(const math::Vector3 &_ptB) + { + this->pts[1] = _ptB; + } + + /// \brief Set the start and end point of the line segment, assuming that + /// both points have the same height. + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + /// \param[in] _z Z coordinate of both points, + /// by default _z is set to 0. + public: void Set(const double _x1, const double _y1, + const double _x2, const double _y2, + const double _z = 0) + { + this->pts[0].Set(_x1, _y1, _z); + this->pts[1].Set(_x2, _y2, _z); + } + + /// \brief Set the start and end point of the line segment + /// \param[in] _x1 X coordinate of the start point. + /// \param[in] _y1 Y coordinate of the start point. + /// \param[in] _z1 Z coordinate of the start point. + /// \param[in] _x2 X coordinate of the end point. + /// \param[in] _y2 Y coordinate of the end point. + /// \param[in] _z2 Z coordinate of the end point. + public: void Set(const double _x1, const double _y1, + const double _z1, const double _x2, + const double _y2, const double _z2) + { + this->pts[0].Set(_x1, _y1, _z1); + this->pts[1].Set(_x2, _y2, _z2); + } + + /// \brief Get the direction of the line + /// \return The direction vector + public: math::Vector3 Direction() const + { + return (this->pts[1] - this->pts[0]).Normalize(); + } + + /// \brief Get the length of the line + /// \return The length of the line. + public: T Length() const + { + return this->pts[0].Distance(this->pts[1]); + } + + /// \brief Get the shortest line between this line and the + /// provided line. + /// + /// In the case when the two lines are parallel, we choose the first + /// point of this line and the closest point in the provided line. + /// \param[in] _line Line to compare against this. + /// \param[out] _result The shortest line between _line and this. + /// \param[in] _epsilon Error tolerance. + /// \return True if a solution was found. False if a solution is not + /// possible. + public: bool Distance(const Line3 &_line, Line3 &_result, + const double _epsilon = 1e-6) const + { + Vector3 p13 = this->pts[0] - _line[0]; + Vector3 p43 = _line[1] - _line[0]; + + if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon && + std::abs(p43.Z()) < _epsilon) + { + return false; + } + + Vector3 p21 = this->pts[1] - this->pts[0]; + + if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon && + std::abs(p21.Z()) < _epsilon) + { + return false; + } + + double d1343 = p13.Dot(p43); + double d4321 = p43.Dot(p21); + double d1321 = p13.Dot(p21); + double d4343 = p43.Dot(p43); + double d2121 = p21.Dot(p21); + + double denom = d2121 * d4343 - d4321 * d4321; + + // In this case, we choose the first point in this line, + // and the closest point in the provided line. + if (std::abs(denom) < _epsilon) + { + double d1 = this->pts[0].Distance(_line[0]); + double d2 = this->pts[0].Distance(_line[1]); + + double d3 = this->pts[1].Distance(_line[0]); + double d4 = this->pts[1].Distance(_line[1]); + + if (d1 <= d2 && d1 <= d3 && d1 <= d4) + { + _result.SetA(this->pts[0]); + _result.SetB(_line[0]); + } + else if (d2 <= d3 && d2 <= d4) + { + _result.SetA(this->pts[0]); + _result.SetB(_line[1]); + } + else if (d3 <= d4) + { + _result.SetA(this->pts[1]); + _result.SetB(_line[0]); + } + else + { + _result.SetA(this->pts[1]); + _result.SetB(_line[1]); + } + + return true; + } + + double numer = d1343 * d4321 - d1321 * d4343; + + double mua = clamp(numer / denom, 0.0, 1.0); + double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0); + + _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub)); + + return true; + } + + /// \brief Calculate shortest distance between line and point + /// \param[in] _pt Point which we are measuring distance to. + /// \returns Distance from point to line. + public: T Distance(const Vector3 &_pt) + { + auto line = this->pts[1] - this->pts[0]; + auto ptTo0 = _pt - this->pts[0]; + auto ptTo1 = _pt - this->pts[1]; + + // Point is projected beyond pt0 or the line has length 0 + if (ptTo0.Dot(line) <= 0.0) + { + return ptTo0.Length(); + } + + // Point is projected beyond pt1 + if (ptTo1.Dot(line) >= 0.0) + { + return ptTo1.Length(); + } + + // Distance to point projected onto line + // line.Length() will have to be > 0 at this point otherwise it would + // return at line 244. + auto d = ptTo0.Cross(line); + auto lineLength = line.Length(); + assert(lineLength > 0); + return d.Length() / lineLength; + } + + /// \brief Check if this line intersects the given line segment. + /// \param[in] _line The line to check for intersection. + /// \param[in] _epsilon The error bounds within which the intersection + /// check will return true. + /// \return True if an intersection was found. + public: bool Intersect(const Line3 &_line, + double _epsilon = 1e-6) const + { + static math::Vector3 ignore; + return this->Intersect(_line, ignore, _epsilon); + } + + /// \brief Test if this line and the given line are coplanar. + /// \param[in] _line Line to check against. + /// \param[in] _epsilon The error bounds within which the + /// check will return true. + /// \return True if the two lines are coplanar. + public: bool Coplanar(const Line3 &_line, + const double _epsilon = 1e-6) const + { + return std::abs((_line[0] - this->pts[0]).Dot( + (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0]))) + <= _epsilon; + } + + /// \brief Test if this line and the given line are parallel. + /// \param[in] _line Line to check against. + /// \param[in] _epsilon The error bounds within which the + /// check will return true. + /// \return True if the two lines are parallel. + public: bool Parallel(const Line3 &_line, + const double _epsilon = 1e-6) const + { + return (this->pts[1] - this->pts[0]).Cross( + _line[1] - _line[0]).Length() <= _epsilon; + } + + /// \brief Check if this line intersects the given line segment. The + /// point of intersection is returned in the _pt parameter. + /// \param[in] _line The line to check for intersection. + /// \param[out] _pt The point of intersection. This value is only + /// valid if the return value is true. + /// \param[in] _epsilon The error bounds within which the intersection + /// check will return true. + /// \return True if an intersection was found. + public: bool Intersect(const Line3 &_line, math::Vector3 &_pt, + double _epsilon = 1e-6) const + { + // Handle special case when lines are parallel + if (this->Parallel(_line, _epsilon)) + { + // Check if _line's starting point is on the line. + if (this->Within(_line[0], _epsilon)) + { + _pt = _line[0]; + return true; + } + // Check if _line's ending point is on the line. + else if (this->Within(_line[1], _epsilon)) + { + _pt = _line[1]; + return true; + } + // Otherwise return false. + else + return false; + } + + // Get the line that is the shortest distance between this and _line + math::Line3 distLine; + this->Distance(_line, distLine, _epsilon); + + // If the length of the line is less than epsilon, then they + // intersect. + if (distLine.Length() < _epsilon) + { + _pt = distLine[0]; + return true; + } + + return false; + } + + /// \brief Check if the given point is between the start and end + /// points of the line segment. + /// \param[in] _pt Point to check. + /// \param[in] _epsilon The error bounds within which the within + /// check will return true. + /// \return True if the point is on the segement. + public: bool Within(const math::Vector3 &_pt, + double _epsilon = 1e-6) const + { + return _pt.X() <= std::max(this->pts[0].X(), + this->pts[1].X()) + _epsilon && + _pt.X() >= std::min(this->pts[0].X(), + this->pts[1].X()) - _epsilon && + _pt.Y() <= std::max(this->pts[0].Y(), + this->pts[1].Y()) + _epsilon && + _pt.Y() >= std::min(this->pts[0].Y(), + this->pts[1].Y()) - _epsilon && + _pt.Z() <= std::max(this->pts[0].Z(), + this->pts[1].Z()) + _epsilon && + _pt.Z() >= std::min(this->pts[0].Z(), + this->pts[1].Z()) - _epsilon; + } + + /// \brief Equality operator. + /// \param[in] _line Line to compare for equality. + /// \return True if the given line is equal to this line + public: bool operator==(const Line3 &_line) const + { + return this->pts[0] == _line[0] && this->pts[1] == _line[1]; + } + + /// \brief Inequality operator. + /// \param[in] _line Line to compare for inequality. + /// \return True if the given line is not to this line + public: bool operator!=(const Line3 &_line) const + { + return !(*this == _line); + } + + /// \brief Get the start or end point. + /// \param[in] _index 0 = start point, 1 = end point. The _index + /// parameter is clamped to the range [0, 1]. + public: math::Vector3 operator[](const size_t _index) const + { + return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; + } + + /// \brief Stream extraction operator + /// \param[in] _out output stream + /// \param[in] _line Line3 to output + /// \return The stream + public: friend std::ostream &operator<<( + std::ostream &_out, const Line3 &_line) + { + _out << _line[0] << " " << _line[1]; + return _out; + } + + /// \brief Assignment operator + /// \param[in] _line a new value + /// \return this + public: Line3 &operator=(const Line3 &_line) + { + this->pts[0] = _line[0]; + this->pts[1] = _line[1]; + + return *this; + } + + /// \brief Vector for storing the start and end points of the line + private: math::Vector3 pts[2]; + }; + + typedef Line3 Line3i; + typedef Line3 Line3d; + typedef Line3 Line3f; + } + } +} +#endif diff --git a/include/gz/math/MassMatrix3.hh b/include/gz/math/MassMatrix3.hh new file mode 100644 index 000000000..e4fba8631 --- /dev/null +++ b/include/gz/math/MassMatrix3.hh @@ -0,0 +1,1280 @@ +/* + * Copyright (C) 2015 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_MASSMATRIX3_HH_ +#define GZ_MATH_MASSMATRIX3_HH_ + +#include +#include +#include +#include + +#include +#include "gz/math/Helpers.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Vector2.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Matrix3.hh" + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class MassMatrix3 MassMatrix3.hh ignition/math/MassMatrix3.hh + /// \brief A class for inertial information about a rigid body + /// consisting of the scalar mass and a 3x3 symmetric moment + /// of inertia matrix stored as two Vector3's. + template + class MassMatrix3 + { + /// \brief Default Constructor, which inializes the mass and moments + /// to zero. + public: MassMatrix3() : mass(0) + {} + + /// \brief Constructor. + /// \param[in] _mass Mass value in kg if using metric. + /// \param[in] _ixxyyzz Diagonal moments of inertia. + /// \param[in] _ixyxzyz Off-diagonal moments of inertia + public: MassMatrix3(const T &_mass, + const Vector3 &_ixxyyzz, + const Vector3 &_ixyxzyz) + : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz) + {} + + /// \brief Copy constructor. + /// \param[in] _m MassMatrix3 element to copy + public: MassMatrix3(const MassMatrix3 &_m) + : mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()), + Ixyxzyz(_m.OffDiagonalMoments()) + {} + + /// \brief Destructor. + public: virtual ~MassMatrix3() {} + + /// \brief Set the mass. + /// \param[in] _m New mass value. + /// \return True if the MassMatrix3 is valid. + /// \deprecated bool SetMass(const T &_m) + public: bool IGN_DEPRECATED(5.0) Mass(const T &_m) + { + return this->SetMass(_m); + } + + /// \brief Set the mass. + /// \param[in] _m New mass value. + /// \return True if the MassMatrix3 is valid. + public: bool SetMass(const T &_m) + { + this->mass = _m; + return this->IsValid(); + } + + /// \brief Get the mass + /// \return The mass value + public: T Mass() const + { + return this->mass; + } + + /// \brief Set the moment of inertia matrix. + /// \param[in] _ixx X second moment of inertia (MOI) about x axis. + /// \param[in] _iyy Y second moment of inertia about y axis. + /// \param[in] _izz Z second moment of inertia about z axis. + /// \param[in] _ixy XY inertia. + /// \param[in] _ixz XZ inertia. + /// \param[in] _iyz YZ inertia. + /// \return True if the MassMatrix3 is valid. + /// \deprecated see bool SetInertiaMatrix(const T &, const T &, const T &, + /// const T &, const T &, const T &) + public: bool IGN_DEPRECATED(5.0) InertiaMatrix( + const T &_ixx, const T &_iyy, const T &_izz, + const T &_ixy, const T &_ixz, const T &_iyz) + { + return this->SetInertiaMatrix(_ixx, _iyy, _izz, _ixy, _ixz, _iyz); + } + + /// \brief Set the moment of inertia matrix. + /// \param[in] _ixx X second moment of inertia (MOI) about x axis. + /// \param[in] _iyy Y second moment of inertia about y axis. + /// \param[in] _izz Z second moment of inertia about z axis. + /// \param[in] _ixy XY inertia. + /// \param[in] _ixz XZ inertia. + /// \param[in] _iyz YZ inertia. + /// \return True if the MassMatrix3 is valid. + public: bool SetInertiaMatrix( + const T &_ixx, const T &_iyy, const T &_izz, + const T &_ixy, const T &_ixz, const T &_iyz) + { + this->Ixxyyzz.Set(_ixx, _iyy, _izz); + this->Ixyxzyz.Set(_ixy, _ixz, _iyz); + return this->IsValid(); + } + + /// \brief Get the diagonal moments of inertia (Ixx, Iyy, Izz). + /// \return The diagonal moments. + public: Vector3 DiagonalMoments() const + { + return this->Ixxyyzz; + } + + /// \brief Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz). + /// \return The off-diagonal moments of inertia. + public: Vector3 OffDiagonalMoments() const + { + return this->Ixyxzyz; + } + + /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz). + /// \param[in] _ixxyyzz diagonal moments of inertia + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetDiagonalMoments(const Vector3 &_ixxyyzz) + public: bool IGN_DEPRECATED(5.0) DiagonalMoments( + const Vector3 &_ixxyyzz) + { + return this->SetDiagonalMoments(_ixxyyzz); + } + + /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz). + /// \param[in] _ixxyyzz diagonal moments of inertia + /// \return True if the MassMatrix3 is valid. + public: bool SetDiagonalMoments(const Vector3 &_ixxyyzz) + { + this->Ixxyyzz = _ixxyyzz; + return this->IsValid(); + } + + /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz). + /// \param[in] _ixyxzyz off-diagonal moments of inertia + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) + public: bool IGN_DEPRECATED(5.0) OffDiagonalMoments( + const Vector3 &_ixyxzyz) + { + return this->SetOffDiagonalMoments(_ixyxzyz); + } + + /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz). + /// \param[in] _ixyxzyz off-diagonal moments of inertia + /// \return True if the MassMatrix3 is valid. + public: bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) + { + this->Ixyxzyz = _ixyxzyz; + return this->IsValid(); + } + + /// \brief Get IXX + /// \return IXX value + /// \deprecated See T Ixx() const + public: T IGN_DEPRECATED(5.0) IXX() const + { + return this->Ixx(); + } + + /// \brief Get IXX + /// \return IXX value + public: T Ixx() const + { + return this->Ixxyyzz[0]; + } + + /// \brief Get IYY + /// \return IYY value + /// \deprecated See T Iyy() const + public: T IGN_DEPRECATED(5.0) IYY() const + { + return this->Iyy(); + } + + /// \brief Get IYY + /// \return IYY value + public: T Iyy() const + { + return this->Ixxyyzz[1]; + } + + /// \brief Get IZZ + /// \return IZZ value + /// \deprecated See T Izz() const + public: T IGN_DEPRECATED(5.0) IZZ() const + { + return this->Izz(); + } + + /// \brief Get IZZ + /// \return IZZ value + public: T Izz() const + { + return this->Ixxyyzz[2]; + } + + /// \brief Get IXY + /// \return IXY value + /// \deprecated See T Ixy() const + public: T IGN_DEPRECATED(5.0) IXY() const + { + return this->Ixy(); + } + + /// \brief Get IXY + /// \return IXY value + public: T Ixy() const + { + return this->Ixyxzyz[0]; + } + + /// \brief Get IXZ + /// \return IXZ value + /// \deprecated See T Ixz() const + public: T IGN_DEPRECATED(5.0) IXZ() const + { + return this->Ixz(); + } + + /// \brief Get IXZ + /// \return IXZ value + public: T Ixz() const + { + return this->Ixyxzyz[1]; + } + + /// \brief Get IYZ + /// \return IYZ value + /// \deprecated See T Iyz() const + public: T IGN_DEPRECATED(5.0) IYZ() const + { + return this->Iyz(); + } + + /// \brief Get IYZ + /// \return IYZ value + public: T Iyz() const + { + return this->Ixyxzyz[2]; + } + + /// \brief Set IXX + /// \param[in] _v IXX value + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetIxx(const T &_v) + public: bool IGN_DEPRECATED(5.0) IXX(const T &_v) + { + return this->SetIxx(_v); + } + + /// \brief Set IXX + /// \param[in] _v IXX value + /// \return True if the MassMatrix3 is valid. + public: bool SetIxx(const T &_v) + { + this->Ixxyyzz.X(_v); + return this->IsValid(); + } + + /// \brief Set IYY + /// \param[in] _v IYY value + /// \return True if the MassMatrix3 is valid. + /// \deprecated see bool SetIyy(const T &_v) + public: bool IGN_DEPRECATED(5.0) IYY(const T &_v) + { + return this->SetIyy(_v); + } + + /// \brief Set IYY + /// \param[in] _v IYY value + /// \return True if the MassMatrix3 is valid. + public: bool SetIyy(const T &_v) + { + this->Ixxyyzz.Y(_v); + return this->IsValid(); + } + + /// \brief Set IZZ + /// \param[in] _v IZZ value + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetIzz(const T &_v) + public: bool IGN_DEPRECATED(5.0) IZZ(const T &_v) + { + return this->SetIzz(_v); + } + + /// \brief Set IZZ + /// \param[in] _v IZZ value + /// \return True if the MassMatrix3 is valid. + public: bool SetIzz(const T &_v) + { + this->Ixxyyzz.Z(_v); + return this->IsValid(); + } + + /// \brief Set IXY + /// \param[in] _v IXY value + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetIxy(const T &_v) + public: bool IGN_DEPRECATED(5.0) IXY(const T &_v) + { + return this->SetIxy(_v); + } + + /// \brief Set IXY + /// \param[in] _v IXY value + /// \return True if the MassMatrix3 is valid. + public: bool SetIxy(const T &_v) + { + this->Ixyxzyz.X(_v); + return this->IsValid(); + } + + /// \brief Set IXZ + /// \param[in] _v IXZ value + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetIxz(const T &_v) + public: bool IGN_DEPRECATED(5.0) IXZ(const T &_v) + { + return this->SetIxz(_v); + } + + /// \brief Set IXZ + /// \param[in] _v IXZ value + /// \return True if the MassMatrix3 is valid. + public: bool SetIxz(const T &_v) + { + this->Ixyxzyz.Y(_v); + return this->IsValid(); + } + + /// \brief Set IYZ + /// \param[in] _v IYZ value + /// \return True if the MassMatrix3 is valid. + /// \deprecated See bool SetIyz(const T &_v) + public: bool IGN_DEPRECATED(5.0) IYZ(const T &_v) + { + return this->SetIyz(_v); + } + + /// \brief Set IYZ + /// \param[in] _v IYZ value + /// \return True if the MassMatrix3 is valid. + public: bool SetIyz(const T &_v) + { + this->Ixyxzyz.Z(_v); + return this->IsValid(); + } + + /// \brief returns Moments of Inertia as a Matrix3 + /// \return Moments of Inertia as a Matrix3 + /// \deprecated See Matrix3 Moi() const + public: Matrix3 IGN_DEPRECATED(5.0) MOI() const + { + return this->Moi(); + } + + /// \brief returns Moments of Inertia as a Matrix3 + /// \return Moments of Inertia as a Matrix3 + public: Matrix3 Moi() const + { + return Matrix3( + this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1], + this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2], + this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]); + } + + /// \brief Sets Moments of Inertia (MOI) from a Matrix3. + /// Symmetric component of input matrix is used by averaging + /// off-axis terms. + /// \param[in] _moi Moments of Inertia as a Matrix3 + /// \return True if the MassMatrix3 is valid. + /// \deprecated See SetMoi(const Matrix3 &_moi) + public: bool IGN_DEPRECATED(5.0) MOI(const Matrix3 &_moi) + { + return this->SetMoi(_moi); + } + + /// \brief Sets Moments of Inertia (MOI) from a Matrix3. + /// Symmetric component of input matrix is used by averaging + /// off-axis terms. + /// \param[in] _moi Moments of Inertia as a Matrix3 + /// \return True if the MassMatrix3 is valid. + public: bool SetMoi(const Matrix3 &_moi) + { + this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2)); + this->Ixyxzyz.Set( + 0.5*(_moi(0, 1) + _moi(1, 0)), + 0.5*(_moi(0, 2) + _moi(2, 0)), + 0.5*(_moi(1, 2) + _moi(2, 1))); + return this->IsValid(); + } + + /// \brief Equal operator. + /// \param[in] _massMatrix MassMatrix3 to copy. + /// \return Reference to this object. + public: MassMatrix3 &operator=(const MassMatrix3 &_massMatrix) + { + this->mass = _massMatrix.Mass(); + this->Ixxyyzz = _massMatrix.DiagonalMoments(); + this->Ixyxzyz = _massMatrix.OffDiagonalMoments(); + + return *this; + } + + /// \brief Equality comparison operator. + /// \param[in] _m MassMatrix3 to copy. + /// \return true if each component is equal within a default tolerance, + /// false otherwise + public: bool operator==(const MassMatrix3 &_m) const + { + return equal(this->mass, _m.Mass()) && + (this->Ixxyyzz == _m.DiagonalMoments()) && + (this->Ixyxzyz == _m.OffDiagonalMoments()); + } + + /// \brief Inequality test operator + /// \param[in] _m MassMatrix3 to test + /// \return True if not equal (using the default tolerance of 1e-6) + public: bool operator!=(const MassMatrix3 &_m) const + { + return !(*this == _m); + } + + /// \brief Verify that inertia values are positive semidefinite + /// + /// \param[in] _tolerance The amount of relative error to accept when + /// checking whether this MassMatrix3 has a valid mass and moment + /// of inertia. Refer to Epsilon() for a description of _tolerance. + /// + /// \return True if mass is nonnegative and moment of inertia matrix + /// is positive semidefinite. The following is how the return value is + /// calculated + /// + /// \code + /// const T epsilon = this->Epsilon(_tolerance); + /// return (this->mass + epsilon >= 0) && + /// (this->IXX() + epsilon >= 0) && + /// (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) + + /// epsilon >= 0) && + /// (this->Moi().Determinant() + epsilon >= 0); + /// \endcode + /// + public: bool IsNearPositive(const T _tolerance = + IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const + { + const T epsilon = this->Epsilon(_tolerance); + + // Check if mass and determinants of all upper left submatrices + // of moment of inertia matrix are positive + return (this->mass >= 0) && + (this->Ixx() + epsilon >= 0) && + (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) + + epsilon >= 0) && + (this->Moi().Determinant() + epsilon >= 0); + } + + /// \brief Verify that inertia values are positive definite + /// + /// \param[in] _tolerance The amount of error to accept when + /// checking whether this MassMatrix3 has a valid mass and moment + /// of inertia. Refer to Epsilon() for a description of _tolerance. + /// + /// \return True if mass is positive and moment of inertia matrix + /// is positive definite. The following is how the return value is + /// calculated + /// + /// \code + /// const T epsilon = this->Epsilon(_tolerance); + /// return (this->mass + epsilon > 0) && + /// (this->IXX() + epsilon > 0) && + /// (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) + + /// epsilon > 0) && + /// (this->Moi().Determinant() + epsilon > 0); + /// \endcode + /// + public: bool IsPositive(const T _tolerance = + IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const + { + const T epsilon = this->Epsilon(_tolerance); + + // Check if mass and determinants of all upper left submatrices + // of moment of inertia matrix are positive + return (this->mass > 0) && + (this->Ixx() + epsilon > 0) && + (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) + + epsilon > 0) && + (this->Moi().Determinant() + epsilon > 0); + } + + /// + /// \brief \copybrief Epsilon(const Vector3&,const T) + /// + /// \param[in] _tolerance A factor that is used to adjust the return + /// value. A value of zero will cause the return value to be zero. + /// A good value is 10, which is also the + /// MASSMATRIX3_DEFAULT_TOLERANCE. + public: T Epsilon(const T _tolerance = + IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const + { + return Epsilon(this->DiagonalMoments(), _tolerance); + } + + /// \brief Get an epsilon value that represents the amount of + /// acceptable error in a MassMatrix3. The epsilon value + /// is related to machine precision multiplied by the largest possible + /// moment of inertia. + /// + /// This function is used by IsValid(), IsNearPositive(), IsPositive(), + /// and ValidMoments(). + /// + /// \param[in] _moments Principal moments of inertia. + /// \param[in] _tolerance A factor that is used to adjust the return + /// value. A value of zero will cause the return value to be zero. + /// A good value is 10, which is also the + /// MASSMATRIX3_DEFAULT_TOLERANCE. + /// + /// \return The epsilon value computed using: + /// + /// \code + /// T maxPossibleMoI = 0.5 * std::abs(_moments.Sum()); + /// return _tolerance * + /// std::numeric_limits::epsilon() * maxPossibleMoI; + /// \endcode + public: static T Epsilon(const Vector3 &_moments, + const T _tolerance = + IGN_MASSMATRIX3_DEFAULT_TOLERANCE) + { + // The following was borrowed heavily from: + // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h + + // Compute the maximum possible moment of inertia, which will be + // used to compute whether the moments are valid. + // + // The maximum moment of inertia is bounded by: + // trace / 3 <= maxPossibleMoi <= trace / 2. + // + // The trace of a matrix is the sum of the coefficients on the + // main diagonal. For a mass matrix, this is equal to + // ixx + iyy + izz, or _moments.Sum() for this function's + // implementation. + // + // It is okay if maxPossibleMoi == zero. + T maxPossibleMoI = 0.5 * std::abs(_moments.Sum()); + + // In order to check validity of the moments we need to use an + // epsilon value that is related to machine precision + // multiplied by the largest possible moment of inertia. + return _tolerance * + std::numeric_limits::epsilon() * maxPossibleMoI; + } + + /// \brief Verify that inertia values are positive semi-definite + /// and satisfy the triangle inequality. + /// + /// \param[in] _tolerance The amount of error to accept when + /// checking whether the MassMatrix3 has a valid mass and moment + /// of inertia. This value is passed on to IsNearPositive() and + /// ValidMoments(), which in turn pass the tolerance value to + /// Epsilon(). Refer to Epsilon() for a description of _tolerance. + /// + /// \return True if IsNearPositive(_tolerance) and + /// ValidMoments(this->PrincipalMoments(), _tolerance) both return true. + public: bool IsValid(const T _tolerance = + IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const + { + return this->IsNearPositive(_tolerance) && + ValidMoments(this->PrincipalMoments(), _tolerance); + } + + /// \brief Verify that principal moments are positive + /// and satisfy the triangle inequality. + /// \param[in] _moments Principal moments of inertia. + /// \param[in] _tolerance The amount of error to accept when + /// checking whether the moments are positive and satisfy the triangle + /// inequality. Refer to Epsilon() for a description of _tolerance. + /// \return True if moments of inertia are positive + /// and satisfy the triangle inequality. The following is how the + /// return value is calculated. + /// + /// \code + /// T epsilon = this->Epsilon(_tolerance); + /// + /// return _moments[0] + epsilon >= 0 && + /// _moments[1] + epsilon >= 0 && + /// _moments[2] + epsilon >= 0 && + /// _moments[0] + _moments[1] + epsilon >= _moments[2] && + /// _moments[1] + _moments[2] + epsilon >= _moments[0] && + /// _moments[2] + _moments[0] + epsilon >= _moments[1]; + /// \endcode + public: static bool ValidMoments(const Vector3 &_moments, + const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE) + { + T epsilon = Epsilon(_moments, _tolerance); + + return _moments[0] + epsilon >= 0 && + _moments[1] + epsilon >= 0 && + _moments[2] + epsilon >= 0 && + _moments[0] + _moments[1] + epsilon >= _moments[2] && + _moments[1] + _moments[2] + epsilon >= _moments[0] && + _moments[2] + _moments[0] + epsilon >= _moments[1]; + } + + /// \brief Compute principal moments of inertia, + /// which are the eigenvalues of the moment of inertia matrix. + /// \param[in] _tol Relative tolerance given by absolute value + /// of _tol. + /// Negative values of _tol are interpreted as a flag that + /// causes principal moments to always be sorted from smallest + /// to largest. + /// \return Principal moments of inertia. + /// If the matrix is already diagonal and _tol is positive, + /// they are returned in the existing order. + /// Otherwise, the moments are sorted from smallest to largest. + public: Vector3 PrincipalMoments(const T _tol = 1e-6) const + { + // Compute tolerance relative to maximum value of inertia diagonal + T tol = _tol * this->Ixxyyzz.Max(); + if (this->Ixyxzyz.Equal(Vector3::Zero, tol)) + { + // Matrix is already diagonalized, return diagonal moments + return this->Ixxyyzz; + } + + // Algorithm based on http://arxiv.org/abs/1306.6291v4 + // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric + // Matrix, by Maarten Kronenburg + Vector3 Id(this->Ixxyyzz); + Vector3 Ip(this->Ixyxzyz); + // b = Ixx + Iyy + Izz + T b = Id.Sum(); + // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2 + T c = Id[0]*Id[1] - std::pow(Ip[0], 2) + + Id[0]*Id[2] - std::pow(Ip[1], 2) + + Id[1]*Id[2] - std::pow(Ip[2], 2); + // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz + T d = Id[0]*std::pow(Ip[2], 2) + + Id[1]*std::pow(Ip[1], 2) + + Id[2]*std::pow(Ip[0], 2) + - Id[0]*Id[1]*Id[2] + - 2*Ip[0]*Ip[1]*Ip[2]; + // p = b^2 - 3c + T p = std::pow(b, 2) - 3*c; + + // At this point, it is important to check that p is not close + // to zero, since its inverse is used to compute delta. + // In equation 4.7, p is expressed as a sum of squares + // that is only zero if the matrix is diagonal + // with identical principal moments. + // This check has no test coverage, since this function returns + // immediately if a diagonal matrix is detected. + if (p < std::pow(tol, 2)) + return b / 3.0 * Vector3::One; + + // q = 2b^3 - 9bc - 27d + T q = 2*std::pow(b, 3) - 9*b*c - 27*d; + + // delta = acos(q / (2 * p^(1.5))) + // additionally clamp the argument to [-1,1] + T delta = acos(clamp(0.5 * q / std::pow(p, 1.5), -1, 1)); + + // sort the moments from smallest to largest + T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0; + T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0; + T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0; + sort3(moment0, moment1, moment2); + return Vector3(moment0, moment1, moment2); + } + + /// \brief Compute rotational offset of principal axes. + /// \param[in] _tol Relative tolerance given by absolute value + /// of _tol. + /// Negative values of _tol are interpreted as a flag that + /// causes principal moments to always be sorted from smallest + /// to largest. + /// \return Quaternion representing rotational offset of principal axes. + /// With a rotation matrix constructed from this quaternion R(q) + /// and a diagonal matrix L with principal moments on the diagonal, + /// the original moment of inertia matrix MOI can be reconstructed + /// with MOI = R(q).Transpose() * L * R(q) + public: Quaternion PrincipalAxesOffset(const T _tol = 1e-6) const + { + Vector3 moments = this->PrincipalMoments(_tol); + // Compute tolerance relative to maximum value of inertia diagonal + T tol = _tol * this->Ixxyyzz.Max(); + if (moments.Equal(this->Ixxyyzz, tol) || + (math::equal(moments[0], moments[1], std::abs(tol)) && + math::equal(moments[0], moments[2], std::abs(tol)))) + { + // matrix is already aligned with principal axes + // or all three moments are approximately equal + // return identity rotation + return Quaternion::Identity; + } + + // Algorithm based on http://arxiv.org/abs/1306.6291v4 + // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric + // Matrix, by Maarten Kronenburg + // A real, symmetric matrix can be diagonalized by an orthogonal matrix + // (due to the finite-dimensional spectral theorem + // https://en.wikipedia.org/wiki/Spectral_theorem + // #Hermitian_maps_and_Hermitian_matrices ), + // and another name for orthogonal matrix is rotation matrix. + // Section 5 of the paper shows how to compute Euler angles + // phi1, phi2, and phi3 that map to a rotation matrix. + // In some cases, there are multiple possible values for a given angle, + // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc. + // Similar variable names are used to the paper so that the paper + // can be used as an additional reference. + + // f1, f2 defined in equations 5.5, 5.6 + Vector2 f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]); + Vector2 f2(this->Ixxyyzz[1] - this->Ixxyyzz[2], + -2*this->Ixyxzyz[2]); + + // Check if two moments are equal, since different equations are used + // The moments vector is already sorted, so just check adjacent values. + Vector2 momentsDiff(moments[0] - moments[1], + moments[1] - moments[2]); + + // index of unequal moment + int unequalMoment = -1; + if (equal(momentsDiff[0], 0, std::abs(tol))) + unequalMoment = 2; + else if (equal(momentsDiff[1], 0, std::abs(tol))) + unequalMoment = 0; + + if (unequalMoment >= 0) + { + // moments[1] is the repeated value + // it is not equal to moments[unequalMoment] + // momentsDiff3 = lambda - lambda3 + T momentsDiff3 = moments[1] - moments[unequalMoment]; + // eq 5.21: + // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3) + // s >= 0 since A_11 is in range [lambda, lambda3] + T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3; + // set phi3 to zero for repeated moments (eq 5.23) + T phi3 = 0; + // phi2 = +- acos(sqrt(s)) + // start with just the positive value + // also clamp the acos argument to prevent NaN's + T phi2 = acos(clamp(ClampedSqrt(s), -1, 1)); + + // The paper defines variables phi11 and phi12 + // which are candidate values of angle phi1. + // phi12 is straightforward to compute as a function of f2 and g2. + // eq 5.25: + Vector2 g2(momentsDiff3 * s, 0); + // combining eq 5.12 and 5.14, and subtracting psi2 + // instead of multiplying by its rotation matrix: + math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); + phi12.Normalize(); + + // The paragraph prior to equation 5.16 describes how to choose + // the candidate value of phi1 based on the length + // of the f1 and f2 vectors. + // * When |f1| != 0 and |f2| != 0, then one should choose the + // value of phi2 so that phi11 = phi12 + // * When |f1| == 0 and f2 != 0, then phi1 = phi12 + // phi11 can be ignored, and either sign of phi2 can be used + // * The case of |f2| == 0 can be ignored at this point in the code + // since having a repeated moment when |f2| == 0 implies that + // the matrix is diagonal. But this function returns a unit + // quaternion for diagonal matrices, so we can assume |f2| != 0 + // See MassMatrix3.ipynb for a more complete discussion. + // + // Since |f2| != 0, we only need to consider |f1| + // * |f1| == 0: phi1 = phi12 + // * |f1| != 0: choose phi2 so that phi11 == phi12 + // In either case, phi1 = phi12, + // and the sign of phi2 must be chosen to make phi11 == phi12 + T phi1 = phi12.Radian(); + + bool f1small = f1.SquaredLength() < std::pow(tol, 2); + if (!f1small) + { + // a: phi2 > 0 + // eq. 5.24 + Vector2 g1a(0, 0.5*momentsDiff3 * sin(2*phi2)); + // combining eq 5.11 and 5.13, and subtracting psi1 + // instead of multiplying by its rotation matrix: + math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol)); + phi11a.Normalize(); + + // b: phi2 < 0 + // eq. 5.24 + Vector2 g1b(0, 0.5*momentsDiff3 * sin(-2*phi2)); + // combining eq 5.11 and 5.13, and subtracting psi1 + // instead of multiplying by its rotation matrix: + math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol)); + phi11b.Normalize(); + + // choose sign of phi2 + // based on whether phi11a or phi11b is closer to phi12 + // use sin and cos to account for angle wrapping + T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2) + + std::pow(cos(phi1) - cos(phi11a.Radian()), 2); + T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2) + + std::pow(cos(phi1) - cos(phi11b.Radian()), 2); + if (errb < erra) + { + phi2 *= -1; + } + } + + // I determined these arguments using trial and error + Quaternion result = Quaternion(-phi1, -phi2, -phi3).Inverse(); + + // Previous equations assume repeated moments are at the beginning + // of the moments vector (moments[0] == moments[1]). + // We have the vectors sorted by size, so it's possible that the + // repeated moments are at the end (moments[1] == moments[2]). + // In this case (unequalMoment == 0), we apply an extra + // rotation that exchanges moment[0] and moment[2] + // Rotation matrix = [ 0 0 1] + // [ 0 1 0] + // [-1 0 0] + // That is equivalent to a 90 degree pitch + if (unequalMoment == 0) + result *= Quaternion(0, IGN_PI_2, 0); + + return result; + } + + // No repeated principal moments + // eq 5.1: + T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2) + +(this->Ixxyyzz[0] - moments[2]) + *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1])) + / ((moments[1] - moments[2]) * (moments[2] - moments[0])); + // value of w depends on v + T w; + if (v < std::abs(tol)) + { + // first sentence after eq 5.4: + // "In the case that v = 0, then w = 1." + w = 1; + } + else + { + // eq 5.2: + w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v) + / ((moments[0] - moments[1]) * v); + } + // initialize values of angle phi1, phi2, phi3 + T phi1 = 0; + // eq 5.3: start with positive value + T phi2 = acos(clamp(ClampedSqrt(v), -1, 1)); + // eq 5.4: start with positive value + T phi3 = acos(clamp(ClampedSqrt(w), -1, 1)); + + // compute g1, g2 for phi2,phi3 >= 0 + // equations 5.7, 5.8 + Vector2 g1( + 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3), + 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2)); + Vector2 g2( + (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v, + (moments[0]-moments[1])*sin(phi2)*sin(2*phi3)); + + // The paragraph prior to equation 5.16 describes how to choose + // the candidate value of phi1 based on the length + // of the f1 and f2 vectors. + // * The case of |f1| == |f2| == 0 implies a repeated moment, + // which should not be possible at this point in the code + // * When |f1| != 0 and |f2| != 0, then one should choose the + // value of phi2 so that phi11 = phi12 + // * When |f1| == 0 and f2 != 0, then phi1 = phi12 + // phi11 can be ignored, and either sign of phi2, phi3 can be used + // * When |f2| == 0 and f1 != 0, then phi1 = phi11 + // phi12 can be ignored, and either sign of phi2, phi3 can be used + bool f1small = f1.SquaredLength() < std::pow(tol, 2); + bool f2small = f2.SquaredLength() < std::pow(tol, 2); + if (f1small && f2small) + { + // this should never happen + // f1small && f2small implies a repeated moment + // return invalid quaternion + /// \todo Use a mock class to test this line + return Quaternion::Zero; + } + else if (f1small) + { + // use phi12 (equations 5.12, 5.14) + math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); + phi12.Normalize(); + phi1 = phi12.Radian(); + } + else if (f2small) + { + // use phi11 (equations 5.11, 5.13) + math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol)); + phi11.Normalize(); + phi1 = phi11.Radian(); + } + else + { + // check for when phi11 == phi12 + // eqs 5.11, 5.13: + math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol)); + phi11.Normalize(); + // eqs 5.12, 5.14: + math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); + phi12.Normalize(); + T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2) + + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2); + phi1 = phi11.Radian(); + math::Vector2 signsPhi23(1, 1); + // case a: phi2 <= 0 + { + Vector2 g1a = Vector2(1, -1) * g1; + Vector2 g2a = Vector2(1, -1) * g2; + math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol)); + math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol))); + phi11a.Normalize(); + phi12a.Normalize(); + T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2) + + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2); + if (erra < err) + { + err = erra; + phi1 = phi11a.Radian(); + signsPhi23.Set(-1, 1); + } + } + // case b: phi3 <= 0 + { + Vector2 g1b = Vector2(-1, 1) * g1; + Vector2 g2b = Vector2(1, -1) * g2; + math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol)); + math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol))); + phi11b.Normalize(); + phi12b.Normalize(); + T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2) + + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2); + if (errb < err) + { + err = errb; + phi1 = phi11b.Radian(); + signsPhi23.Set(1, -1); + } + } + // case c: phi2,phi3 <= 0 + { + Vector2 g1c = Vector2(-1, -1) * g1; + Vector2 g2c = g2; + math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol)); + math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol))); + phi11c.Normalize(); + phi12c.Normalize(); + T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2) + + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2); + if (errc < err) + { + phi1 = phi11c.Radian(); + signsPhi23.Set(-1, -1); + } + } + + // apply sign changes + phi2 *= signsPhi23[0]; + phi3 *= signsPhi23[1]; + } + + // I determined these arguments using trial and error + return Quaternion(-phi1, -phi2, -phi3).Inverse(); + } + + /// \brief Get dimensions and rotation offset of uniform box + /// with equivalent mass and moment of inertia. + /// To compute this, the Matrix3 is diagonalized. + /// The eigenvalues on the diagonal and the rotation offset + /// of the principal axes are returned. + /// \param[in] _size Dimensions of box aligned with principal axes. + /// \param[in] _rot Rotational offset of principal axes. + /// \param[in] _tol Relative tolerance. + /// \return True if box properties were computed successfully. + public: bool EquivalentBox(Vector3 &_size, + Quaternion &_rot, + const T _tol = 1e-6) const + { + if (!this->IsPositive(0)) + { + // inertia is not positive, cannot compute equivalent box + return false; + } + + Vector3 moments = this->PrincipalMoments(_tol); + if (!ValidMoments(moments)) + { + // principal moments don't satisfy the triangle identity + return false; + } + + // The reason for checking that the principal moments satisfy + // the triangle inequality + // I1 + I2 - I3 >= 0 + // is to ensure that the arguments to sqrt in these equations + // are positive and the box size is real. + _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass)); + _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass)); + _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass)); + + _rot = this->PrincipalAxesOffset(_tol); + + if (_rot == Quaternion::Zero) + { + // _rot is an invalid quaternion + /// \todo Use a mock class to test this line + return false; + } + + return true; + } + + /// \brief Set inertial properties based on a Material and equivalent box. + /// \param[in] _mat Material that specifies a density. Uniform density + /// is used. + /// \param[in] _size Size of equivalent box. + /// \param[in] _rot Rotational offset of equivalent box. + /// \return True if inertial properties were set successfully. + public: bool SetFromBox(const Material &_mat, + const Vector3 &_size, + const Quaternion &_rot = Quaternion::Identity) + { + T volume = _size.X() * _size.Y() * _size.Z(); + return this->SetFromBox(_mat.Density() * volume, _size, _rot); + } + + /// \brief Set inertial properties based on mass and equivalent box. + /// \param[in] _mass Mass to set. + /// \param[in] _size Size of equivalent box. + /// \param[in] _rot Rotational offset of equivalent box. + /// \return True if inertial properties were set successfully. + public: bool SetFromBox(const T _mass, + const Vector3 &_size, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that _mass and _size are strictly positive + // and that quaternion is valid + if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion::Zero) + { + return false; + } + this->SetMass(_mass); + return this->SetFromBox(_size, _rot); + } + + /// \brief Set inertial properties based on equivalent box + /// using the current mass value. + /// \param[in] _size Size of equivalent box. + /// \param[in] _rot Rotational offset of equivalent box. + /// \return True if inertial properties were set successfully. + public: bool SetFromBox(const Vector3 &_size, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that _mass and _size are strictly positive + // and that quaternion is valid + if (this->Mass() <= 0 || _size.Min() <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + + // Diagonal matrix L with principal moments + Matrix3 L; + T x2 = std::pow(_size.X(), 2); + T y2 = std::pow(_size.Y(), 2); + T z2 = std::pow(_size.Z(), 2); + L(0, 0) = this->mass / 12.0 * (y2 + z2); + L(1, 1) = this->mass / 12.0 * (z2 + x2); + L(2, 2) = this->mass / 12.0 * (x2 + y2); + Matrix3 R(_rot); + return this->SetMoi(R * L * R.Transposed()); + } + + /// \brief Set inertial properties based on a Material and equivalent + /// cylinder aligned with Z axis. + /// \param[in] _mat Material that specifies a density. Uniform density + /// is used. + /// \param[in] _length Length of cylinder along Z axis. + /// \param[in] _radius Radius of cylinder. + /// \param[in] _rot Rotational offset of equivalent cylinder. + /// \return True if inertial properties were set successfully. + public: bool SetFromCylinderZ(const Material &_mat, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that density, _radius and _length are strictly positive + // and that quaternion is valid + if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + T volume = IGN_PI * _radius * _radius * _length; + return this->SetFromCylinderZ(_mat.Density() * volume, + _length, _radius, _rot); + } + + /// \brief Set inertial properties based on mass and equivalent cylinder + /// aligned with Z axis. + /// \param[in] _mass Mass to set. + /// \param[in] _length Length of cylinder along Z axis. + /// \param[in] _radius Radius of cylinder. + /// \param[in] _rot Rotational offset of equivalent cylinder. + /// \return True if inertial properties were set successfully. + public: bool SetFromCylinderZ(const T _mass, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity) + { + // Check that _mass, _radius and _length are strictly positive + // and that quaternion is valid + if (_mass <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + this->SetMass(_mass); + return this->SetFromCylinderZ(_length, _radius, _rot); + } + + /// \brief Set inertial properties based on equivalent cylinder + /// aligned with Z axis using the current mass value. + /// \param[in] _length Length of cylinder along Z axis. + /// \param[in] _radius Radius of cylinder. + /// \param[in] _rot Rotational offset of equivalent cylinder. + /// \return True if inertial properties were set successfully. + public: bool SetFromCylinderZ(const T _length, + const T _radius, + const Quaternion &_rot) + { + // Check that _mass and _size are strictly positive + // and that quaternion is valid + if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + + // Diagonal matrix L with principal moments + T radius2 = std::pow(_radius, 2); + Matrix3 L; + L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2)); + L(1, 1) = L(0, 0); + L(2, 2) = this->mass / 2.0 * radius2; + Matrix3 R(_rot); + return this->SetMoi(R * L * R.Transposed()); + } + + /// \brief Set inertial properties based on a material and + /// equivalent sphere. + /// \param[in] _mat Material that specifies a density. Uniform density + /// is used. + /// \param[in] _radius Radius of equivalent, uniform sphere. + /// \return True if inertial properties were set successfully. + public: bool SetFromSphere(const Material &_mat, const T _radius) + { + // Check that the density and _radius are strictly positive + if (_mat.Density() <= 0 || _radius <= 0) + { + return false; + } + + T volume = (4.0/3.0) * IGN_PI * std::pow(_radius, 3); + return this->SetFromSphere(_mat.Density() * volume, _radius); + } + + /// \brief Set inertial properties based on mass and equivalent sphere. + /// \param[in] _mass Mass to set. + /// \param[in] _radius Radius of equivalent, uniform sphere. + /// \return True if inertial properties were set successfully. + public: bool SetFromSphere(const T _mass, const T _radius) + { + // Check that _mass and _radius are strictly positive + if (_mass <= 0 || _radius <= 0) + { + return false; + } + this->SetMass(_mass); + return this->SetFromSphere(_radius); + } + + /// \brief Set inertial properties based on equivalent sphere + /// using the current mass value. + /// \param[in] _radius Radius of equivalent, uniform sphere. + /// \return True if inertial properties were set successfully. + public: bool SetFromSphere(const T _radius) + { + // Check that _mass and _radius are strictly positive + if (this->Mass() <= 0 || _radius <= 0) + { + return false; + } + + // Diagonal matrix L with principal moments + T radius2 = std::pow(_radius, 2); + Matrix3 L; + L(0, 0) = 0.4 * this->mass * radius2; + L(1, 1) = 0.4 * this->mass * radius2; + L(2, 2) = 0.4 * this->mass * radius2; + return this->SetMoi(L); + } + + /// \brief Square root of positive numbers, otherwise zero. + /// \param[in] _x Number to be square rooted. + /// \return sqrt(_x) if _x > 0, otherwise 0 + private: static inline T ClampedSqrt(const T &_x) + { + if (_x <= 0) + return 0; + return sqrt(_x); + } + + /// \brief Angle formed by direction of a Vector2. + /// \param[in] _v Vector whose direction is to be computed. + /// \param[in] _eps Minimum length of vector required for computing angle. + /// \return Angle formed between vector and X axis, + /// or zero if vector has length less than 1e-6. + private: static T Angle2(const Vector2 &_v, const T _eps = 1e-6) + { + if (_v.SquaredLength() < std::pow(_eps, 2)) + return 0; + return atan2(_v[1], _v[0]); + } + + /// \brief Mass of the object. Default is 0.0. + private: T mass; + + /// \brief Principal moments of inertia. Default is (0.0 0.0 0.0) + /// These Moments of Inertia are specified in the local frame. + /// Where Ixxyyzz.x is Ixx, Ixxyyzz.y is Iyy and Ixxyyzz.z is Izz. + private: Vector3 Ixxyyzz; + + /// \brief Product moments of inertia. Default is (0.0 0.0 0.0) + /// These MOI off-diagonals are specified in the local frame. + /// Where Ixyxzyz.x is Ixy, Ixyxzyz.y is Ixz and Ixyxzyz.z is Iyz. + private: Vector3 Ixyxzyz; + }; + + typedef MassMatrix3 MassMatrix3d; + typedef MassMatrix3 MassMatrix3f; + } + } +} +#endif diff --git a/include/ignition/math/MassMatrix3.ipynb b/include/gz/math/MassMatrix3.ipynb similarity index 100% rename from include/ignition/math/MassMatrix3.ipynb rename to include/gz/math/MassMatrix3.ipynb diff --git a/include/gz/math/Material.hh b/include/gz/math/Material.hh new file mode 100644 index 000000000..add7fe13f --- /dev/null +++ b/include/gz/math/Material.hh @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2018 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_MATERIAL_HH_ +#define GZ_MATH_MATERIAL_HH_ + +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // Forward declarations + class MaterialPrivate; + + /// \brief Contains information about a single material. + /// + /// Steel, wood, and iron are examples of materials. This class + /// allows you to create custom materials, or use built-in materials. + /// The list of built-in materials can be found in the ::MaterialType + /// enum. + /// + /// This class will replace the + /// [MaterialDensity class](https://github.com/ignitionrobotics/ign-common/blob/ign-common1/include/ignition/common/MaterialDensity.hh) + /// found in the Ignition Common library, which was at version 1 at the + /// time of this writing. + /// + /// **How to create a wood material:** + /// + /// ~~~ + /// Material mat(MaterialType::WOOD); + /// std::cout << "The density of " << mat.Name() << " is " + /// << mat.Density() << std::endl; + /// ~~~ + /// + /// **How to create a custom material:** + /// + /// ~~~ + /// Material mat; + /// mat.SetDensity(12.23); + /// mat.SetName("my_material"); + /// std::cout << "The density of " << mat.Name() is " + /// << mat.Density() << std::endl; + /// ~~~ + class IGNITION_MATH_VISIBLE Material + { + /// \brief Constructor. + public: Material(); + + /// \brief Construct a material based on a type. + /// \param[in] _type Built-in type to create. + public: explicit Material(const MaterialType _type); + + /// \brief Construct a material based on a type name. + /// \param[in] _typename Name of the built-in type to create. String + /// names are listed in the ::MaterialType documentation. + public: explicit Material(const std::string &_typename); + + /// \brief Construct a material based on a density value. + /// \param[in] _density Material density. + public: explicit Material(const double _density); + + /// \brief Copy constructor. + /// \param[in] _material Material to copy. + public: Material(const Material &_material); + + /// \brief Move constructor. + /// \param[in] _material Material to move. The _material object will + /// have default values after the move. + public: Material(Material &&_material); + + /// \brief Destructor. + public: ~Material(); + + /// \brief Get all the built-in materials. + /// \return A map of all the materials. The map's key is + /// material type and the map's value is the material object. + public: static const std::map &Predefined(); + + /// \brief Set this Material to the built-in Material with + /// the nearest density value within _epsilon. If a built-in material + /// could not be found, then this Material is not changed. + /// \param[in] _value Density value of entry to match. + /// \param[in] _epsilon Allowable range of difference between _value, + /// and a material's density. + public: void SetToNearestDensity( + const double _value, + const double _epsilon = std::numeric_limits::max()); + + /// \brief Copy operator. + /// \param[in] _material Material to copy. + /// \return Reference to this material. + public: Material &operator=(const Material &_material); + + /// \brief Move operator. + /// \param[in] _material Material to move. The _material object will + /// contain default values after this move. + /// \return Reference to this Material. + public: Material &operator=(Material &&_material); + + /// \brief Equality operator. This compares type and density values. + /// \param[in] _material Material to evaluate this object against. + /// \return True if this material is equal to the given _material. + public: bool operator==(const Material &_material) const; + + /// \brief Inequality operator. This compares type and density values. + /// \param[in] _material Material to evaluate this object against. + /// \return True if this material is not equal to the given _material. + public: bool operator!=(const Material &_material) const; + + /// \brief Get the material's type. + /// \return The material's type. + public: MaterialType Type() const; + + /// \brief Set the material's type. This will only set the type value. + /// Other properties, such as density, will not be changed. + /// \param[in] _type The material's type. + public: void SetType(const MaterialType _type); + + /// \brief Get the name of the material. This will match the enum type + /// names used in ::MaterialType, but in lowercase, if a built-in + /// material is used. + /// \return The material's name. + /// \sa void SetName(const std::string &_name) + public: std::string Name() const; + + /// \brief Set the name of the material. + /// \param[in] _name The material's name. + /// \sa std::string Name() const + public: void SetName(const std::string &_name); + + /// \brief Get the density value of the material in kg/m^3. + /// \return The density of this material in kg/m^3. + public: double Density() const; + + /// \brief Set the density value of the material in kg/m^3. + /// \param[in] _density The density of this material in kg/m^3. + public: void SetDensity(const double _density); + + /// \brief Private data pointer. + private: MaterialPrivate *dataPtr = nullptr; + }; + } + } +} +#endif diff --git a/include/gz/math/MaterialType.hh b/include/gz/math/MaterialType.hh new file mode 100644 index 000000000..046cc23d3 --- /dev/null +++ b/include/gz/math/MaterialType.hh @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2018 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_MATERIALTYPES_HH_ +#define GZ_MATH_MATERIALTYPES_HH_ + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \enum MaterialType + /// \brief This enum lists the supported material types. A value can be + /// used to create a Material instance. + /// Source: https://en.wikipedia.org/wiki/Density + /// \sa Material + // Developer Note: When modifying this enum, make sure to also modify + // the kMaterials map in src/MaterialTypes.hh. + enum class MaterialType + { + /// \brief Styrofoam, density = 75.0 kg/m^3 + /// String name = "styrofoam" + STYROFOAM = 0, + + /// \brief Pine, density = 373.0 kg/m^3 + /// String name = "pine" + PINE, + + /// \brief Wood, density = 700.0 kg/m^3 + /// String name = "wood" + WOOD, + + /// \brief Oak, density = 710.0 kg/m^3 + /// String name = "oak" + OAK, + + /// \brief Plastic, density = 1175.0 kg/m^3 + /// String name = "plastic" + PLASTIC, + + /// \brief Concrete, density = 2000.0 kg/m^3 + /// String name = "concrete" + CONCRETE, + + /// \brief Aluminum, density = 2700.0 kg/m^3 + /// String name = "aluminum" + ALUMINUM, + + /// \brief Steel alloy, density = 7600.0 kg/m^3 + /// String name = "steel_alloy" + STEEL_ALLOY, + + /// \brief Stainless steel, density = 7800.0 kg/m^3 + /// String name = "steel_stainless" + STEEL_STAINLESS, + + /// \brief Iron, density = 7870.0 kg/m^3 + /// String name = "iron" + IRON, + + /// \brief Brass, density = 8600.0 kg/m^3 + /// String name = "brass" + BRASS, + + /// \brief Copper, density = 8940.0 kg/m^3 + /// String name = "copper" + COPPER, + + /// \brief Tungsten, density = 19300.0 kg/m^3 + /// String name = "tungsten" + TUNGSTEN, + + /// \brief Represents an invalid or unknown material. + // This value should always be last in the enum; it is used in + // MaterialDensity_TEST. + UNKNOWN_MATERIAL + }; + } + } +} +#endif diff --git a/include/gz/math/Matrix3.hh b/include/gz/math/Matrix3.hh new file mode 100644 index 000000000..ac64513fe --- /dev/null +++ b/include/gz/math/Matrix3.hh @@ -0,0 +1,554 @@ +/* + * Copyright (C) 2012 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_MATRIX3_HH_ +#define GZ_MATH_MATRIX3_HH_ + +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + template class Quaternion; + + /// \class Matrix3 Matrix3.hh ignition/math/Matrix3.hh + /// \brief A 3x3 matrix class + template + class Matrix3 + { + /// \brief Identity matrix + public: static const Matrix3 Identity; + + /// \brief Zero matrix + public: static const Matrix3 Zero; + + /// \brief Constructor + public: Matrix3() + { + std::memset(this->data, 0, sizeof(this->data[0][0])*9); + } + + /// \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 + /// \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] _v10 Row 1, Col 0 value + /// \param[in] _v11 Row 1, Col 1 value + /// \param[in] _v12 Row 1, Col 2 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 + public: Matrix3(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22) + { + this->data[0][0] = _v00; + this->data[0][1] = _v01; + this->data[0][2] = _v02; + this->data[1][0] = _v10; + this->data[1][1] = _v11; + this->data[1][2] = _v12; + this->data[2][0] = _v20; + this->data[2][1] = _v21; + this->data[2][2] = _v22; + } + + /// \brief Construct Matrix3 from a quaternion. + /// \param[in] _q Quaternion. + public: explicit Matrix3(const Quaternion &_q) + { + Quaternion qt = _q; + qt.Normalize(); + this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(), + 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(), + 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(), + 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(), + 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(), + 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(), + 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(), + 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(), + 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y()); + } + + /// \brief Desctructor + public: virtual ~Matrix3() {} + + /// \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 + /// \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] _v20 Row 2, Col 0 value + /// \param[in] _v21 Row 2, Col 1 value + /// \param[in] _v22 Row 2, Col 2 value + public: void Set(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22) + { + this->data[0][0] = _v00; + this->data[0][1] = _v01; + this->data[0][2] = _v02; + this->data[1][0] = _v10; + this->data[1][1] = _v11; + this->data[1][2] = _v12; + this->data[2][0] = _v20; + this->data[2][1] = _v21; + 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) + { + this->Col(0, _xAxis); + this->Col(1, _yAxis); + this->Col(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) + { + T c = cos(_angle); + T s = sin(_angle); + T C = 1-c; + + this->data[0][0] = _axis.X()*_axis.X()*C + c; + this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s; + this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s; + + this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s; + this->data[1][1] = _axis.Y()*_axis.Y()*C + c; + this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s; + + this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s; + this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s; + this->data[2][2] = _axis.Z()*_axis.Z()*C + c; + } + + /// \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 From2Axes(const Vector3 &_v1, const Vector3 &_v2) + { + const T _v1LengthSquared = _v1.SquaredLength(); + if (_v1LengthSquared <= 0.0) + { + // zero vector - we can't handle this + this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); + return; + } + + const T _v2LengthSquared = _v2.SquaredLength(); + if (_v2LengthSquared <= 0.0) + { + // zero vector - we can't handle this + this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); + return; + } + + const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared); + if (fabs(dot - 1.0) <= 1e-6) + { + // the vectors are parallel + this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); + return; + } + else if (fabs(dot + 1.0) <= 1e-6) + { + // the vectors are opposite + this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1); + return; + } + + const Vector3 cross = _v1.Cross(_v2).Normalize(); + + this->Axis(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. + public: void Col(unsigned int _c, const Vector3 &_v) + { + unsigned int c = clamp(_c, 0u, 2u); + + this->data[0][c] = _v.X(); + this->data[1][c] = _v.Y(); + this->data[2][c] = _v.Z(); + } + + /// \brief Equal operator. this = _mat + /// \param _mat Incoming matrix + /// \return itself + 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 + public: Matrix3 operator-(const Matrix3 &_m) const + { + return Matrix3( + this->data[0][0] - _m(0, 0), + this->data[0][1] - _m(0, 1), + this->data[0][2] - _m(0, 2), + this->data[1][0] - _m(1, 0), + this->data[1][1] - _m(1, 1), + this->data[1][2] - _m(1, 2), + this->data[2][0] - _m(2, 0), + this->data[2][1] - _m(2, 1), + this->data[2][2] - _m(2, 2)); + } + + /// \brief returns the element wise sum of two matrices + public: Matrix3 operator+(const Matrix3 &_m) const + { + return Matrix3( + this->data[0][0]+_m(0, 0), + this->data[0][1]+_m(0, 1), + this->data[0][2]+_m(0, 2), + this->data[1][0]+_m(1, 0), + this->data[1][1]+_m(1, 1), + this->data[1][2]+_m(1, 2), + this->data[2][0]+_m(2, 0), + this->data[2][1]+_m(2, 1), + this->data[2][2]+_m(2, 2)); + } + + /// \brief returns the element wise scalar multiplication + public: Matrix3 operator*(const T &_s) const + { + return Matrix3( + _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2], + _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2], + _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]); + } + + /// \brief Matrix multiplication operator + /// \param[in] _m Matrix3 to multiply + /// \return product of this * _m + public: Matrix3 operator*(const Matrix3 &_m) const + { + return Matrix3( + // first row + this->data[0][0]*_m(0, 0)+ + this->data[0][1]*_m(1, 0)+ + this->data[0][2]*_m(2, 0), + + this->data[0][0]*_m(0, 1)+ + this->data[0][1]*_m(1, 1)+ + this->data[0][2]*_m(2, 1), + + this->data[0][0]*_m(0, 2)+ + this->data[0][1]*_m(1, 2)+ + this->data[0][2]*_m(2, 2), + + // second row + this->data[1][0]*_m(0, 0)+ + this->data[1][1]*_m(1, 0)+ + this->data[1][2]*_m(2, 0), + + this->data[1][0]*_m(0, 1)+ + this->data[1][1]*_m(1, 1)+ + this->data[1][2]*_m(2, 1), + + this->data[1][0]*_m(0, 2)+ + this->data[1][1]*_m(1, 2)+ + this->data[1][2]*_m(2, 2), + + // third row + this->data[2][0]*_m(0, 0)+ + this->data[2][1]*_m(1, 0)+ + this->data[2][2]*_m(2, 0), + + this->data[2][0]*_m(0, 1)+ + this->data[2][1]*_m(1, 1)+ + this->data[2][2]*_m(2, 1), + + this->data[2][0]*_m(0, 2)+ + this->data[2][1]*_m(1, 2)+ + this->data[2][2]*_m(2, 2)); + } + + /// \brief Multiplication operator with Vector3 on the right + /// treated like a column vector. + /// \param _vec Vector3 + /// \return Resulting vector from multiplication + public: Vector3 operator*(const Vector3 &_vec) const + { + return Vector3( + this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() + + this->data[0][2]*_vec.Z(), + this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() + + this->data[1][2]*_vec.Z(), + this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() + + this->data[2][2]*_vec.Z()); + } + + /// \brief Matrix multiplication operator for scaling. + /// \param[in] _s Scaling factor. + /// \param[in] _m Input matrix. + /// \return A scaled matrix. + public: friend inline Matrix3 operator*(T _s, const Matrix3 &_m) + { + return _m * _s; + } + + /// \brief Matrix left multiplication operator for Vector3. + /// Treats the Vector3 like a row vector multiplying the matrix + /// from the left. + /// \param[in] _v Input vector. + /// \param[in] _m Input matrix. + /// \return The product vector. + public: friend inline Vector3 operator*(const Vector3 &_v, + const Matrix3 &_m) + { + return Vector3( + _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(), + _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(), + _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z()); + } + + /// \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 Matrix3 &_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[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[2][0], _m(2, 0), _tol) + && equal(this->data[2][1], _m(2, 1), _tol) + && 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) + public: bool operator==(const Matrix3 &_m) const + { + return this->Equal(_m, static_cast(1e-6)); + } + + /// \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) + { + 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) + public: bool operator!=(const Matrix3 &_m) const + { + return !(*this == _m); + } + + /// \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 + { + 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 + /// \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 T &operator()(size_t _row, size_t _col) + { + return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; + } + + /// \brief Return the determinant of the matrix + /// \return Determinant of this matrix. + public: T Determinant() const + { + T t0 = this->data[2][2]*this->data[1][1] + - this->data[2][1]*this->data[1][2]; + + T t1 = -(this->data[2][2]*this->data[1][0] + -this->data[2][0]*this->data[1][2]); + + T t2 = this->data[2][1]*this->data[1][0] + - this->data[2][0]*this->data[1][1]; + + return t0 * this->data[0][0] + + t1 * this->data[0][1] + + t2 * this->data[0][2]; + } + + /// \brief Return the inverse matrix + /// \return Inverse of this matrix. + public: Matrix3 Inverse() const + { + T t0 = this->data[2][2]*this->data[1][1] - + this->data[2][1]*this->data[1][2]; + + T t1 = -(this->data[2][2]*this->data[1][0] - + this->data[2][0]*this->data[1][2]); + + T t2 = this->data[2][1]*this->data[1][0] - + this->data[2][0]*this->data[1][1]; + + T invDet = 1.0 / (t0 * this->data[0][0] + + t1 * this->data[0][1] + + t2 * this->data[0][2]); + + return invDet * Matrix3( + t0, + - (this->data[2][2] * this->data[0][1] - + this->data[2][1] * this->data[0][2]), + + (this->data[1][2] * this->data[0][1] - + this->data[1][1] * this->data[0][2]), + t1, + + (this->data[2][2] * this->data[0][0] - + this->data[2][0] * this->data[0][2]), + - (this->data[1][2] * this->data[0][0] - + this->data[1][0] * this->data[0][2]), + t2, + - (this->data[2][1] * this->data[0][0] - + this->data[2][0] * this->data[0][1]), + + (this->data[1][1] * this->data[0][0] - + this->data[1][0] * this->data[0][1])); + } + + /// \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[1][2], this->data[2][1]); + } + + /// \brief Return the transpose of this matrix + /// \return Transpose of this matrix. + public: Matrix3 Transposed() const + { + return Matrix3( + this->data[0][0], this->data[1][0], this->data[2][0], + this->data[0][1], this->data[1][1], this->data[2][1], + 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 + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Matrix3 &_m) + { + _out << precision(_m(0, 0), 6) << " " + << precision(_m(0, 1), 6) << " " + << precision(_m(0, 2), 6) << " " + << precision(_m(1, 0), 6) << " " + << precision(_m(1, 1), 6) << " " + << precision(_m(1, 2), 6) << " " + << precision(_m(2, 0), 6) << " " + << precision(_m(2, 1), 6) << " " + << precision(_m(2, 2), 6); + + return _out; + } + + /// \brief Stream extraction operator + /// \param[in,out] _in input stream + /// \param[out] _m Matrix3 to read values into + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, gz::math::Matrix3 &_m) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + T d[9]; + _in >> d[0] >> d[1] >> d[2] + >> d[3] >> d[4] >> d[5] + >> d[6] >> d[7] >> d[8]; + + if (!_in.fail()) + { + _m.Set(d[0], d[1], d[2], + d[3], d[4], d[5], + d[6], d[7], d[8]); + } + return _in; + } + + /// \brief the 3x3 matrix + private: T data[3][3]; + }; + + template + const Matrix3 Matrix3::Identity( + 1, 0, 0, + 0, 1, 0, + 0, 0, 1); + + template + const Matrix3 Matrix3::Zero( + 0, 0, 0, + 0, 0, 0, + 0, 0, 0); + + typedef Matrix3 Matrix3i; + typedef Matrix3 Matrix3d; + typedef Matrix3 Matrix3f; + } + } +} + +#endif diff --git a/include/gz/math/Matrix4.hh b/include/gz/math/Matrix4.hh new file mode 100644 index 000000000..624f4e799 --- /dev/null +++ b/include/gz/math/Matrix4.hh @@ -0,0 +1,930 @@ +/* + * Copyright (C) 2012 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_MATRIX4_HH_ +#define GZ_MATH_MATRIX4_HH_ + +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Matrix4 Matrix4.hh ignition/math/Matrix4.hh + /// \brief A 4x4 matrix class + template + class Matrix4 + { + /// \brief Identity matrix + public: static const Matrix4 Identity; + + /// \brief Zero matrix + public: static const Matrix4 Zero; + + /// \brief Constructor + public: Matrix4() + { + memset(this->data, 0, sizeof(this->data[0][0])*16); + } + + /// \brief Copy constructor + /// \param _m Matrix to copy + public: Matrix4(const Matrix4 &_m) + { + memcpy(this->data, _m.data, sizeof(this->data[0][0])*16); + } + + /// \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] _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: Matrix4(T _v00, T _v01, T _v02, T _v03, + T _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33) + { + this->Set(_v00, _v01, _v02, _v03, + _v10, _v11, _v12, _v13, + _v20, _v21, _v22, _v23, + _v30, _v31, _v32, _v33); + } + + /// \brief Construct Matrix4 from a quaternion. + /// \param[in] _q Quaternion. + public: explicit Matrix4(const Quaternion &_q) + { + Quaternion qt = _q; + qt.Normalize(); + this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(), + 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(), + 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(), + 0, + + 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(), + 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(), + 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(), + 0, + + 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(), + 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(), + 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(), + 0, + + 0, 0, 0, 1); + } + + /// \brief Construct Matrix4 from a math::Pose3 + /// \param[in] _pose Pose. + public: explicit Matrix4(const Pose3 &_pose) : Matrix4(_pose.Rot()) + { + this->SetTranslation(_pose.Pos()); + } + + /// \brief Destructor + public: virtual ~Matrix4() {} + + /// \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 _v10, T _v11, T _v12, T _v13, + T _v20, T _v21, T _v22, T _v23, + T _v30, T _v31, T _v32, T _v33) + { + this->data[0][0] = _v00; + this->data[0][1] = _v01; + this->data[0][2] = _v02; + this->data[0][3] = _v03; + + this->data[1][0] = _v10; + this->data[1][1] = _v11; + this->data[1][2] = _v12; + this->data[1][3] = _v13; + + this->data[2][0] = _v20; + this->data[2][1] = _v21; + this->data[2][2] = _v22; + this->data[2][3] = _v23; + + this->data[3][0] = _v30; + this->data[3][1] = _v31; + this->data[3][2] = _v32; + this->data[3][3] = _v33; + } + + /// \brief Set the upper-left 3x3 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) + { + T c = cos(_angle); + T s = sin(_angle); + T C = 1-c; + + this->data[0][0] = _axis.X()*_axis.X()*C + c; + this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s; + this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s; + + this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s; + this->data[1][1] = _axis.Y()*_axis.Y()*C + c; + this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s; + + this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s; + this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s; + this->data[2][2] = _axis.Z()*_axis.Z()*C + c; + } + + /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] + /// \param[in] _t Values to set + /// \deprecated Use SetTranslation instead + public: void + IGN_DEPRECATED(4) + Translate(const Vector3 &_t) + { + this->SetTranslation(_t); + } + + /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] + /// \param[in] _t Values to set + public: void SetTranslation(const Vector3 &_t) + { + this->data[0][3] = _t.X(); + this->data[1][3] = _t.Y(); + this->data[2][3] = _t.Z(); + } + + /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] + /// \param[in] _x X translation value. + /// \param[in] _y Y translation value. + /// \param[in] _z Z translation value. + /// \deprecated Use SetTranslation instead + public: void + IGN_DEPRECATED(4) + Translate(T _x, T _y, T _z) + { + this->SetTranslation(_x, _y, _z); + } + + /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] + /// \param[in] _x X translation value. + /// \param[in] _y Y translation value. + /// \param[in] _z Z translation value. + public: void SetTranslation(T _x, T _y, T _z) + { + this->data[0][3] = _x; + this->data[1][3] = _y; + this->data[2][3] = _z; + } + + /// \brief Get the translational values as a Vector3 + /// \return x,y,z translation values + public: Vector3 Translation() const + { + return Vector3(this->data[0][3], this->data[1][3], this->data[2][3]); + } + + /// \brief Get the scale values as a Vector3 + /// \return x,y,z scale values + public: Vector3 Scale() const + { + return Vector3(this->data[0][0], this->data[1][1], this->data[2][2]); + } + + /// \brief Get the rotation as a quaternion + /// \return the rotation + public: Quaternion Rotation() const + { + Quaternion q; + /// algorithm from Ogre::Quaternion source, which in turn is based on + /// Ken Shoemake's article "Quaternion Calculus and Fast Animation". + T trace = this->data[0][0] + this->data[1][1] + this->data[2][2]; + T root; + if (trace > 0) + { + root = sqrt(trace + 1.0); + q.W(root / 2.0); + root = 1.0 / (2.0 * root); + q.X((this->data[2][1] - this->data[1][2]) * root); + q.Y((this->data[0][2] - this->data[2][0]) * root); + q.Z((this->data[1][0] - this->data[0][1]) * root); + } + else + { + static unsigned int s_iNext[3] = {1, 2, 0}; + unsigned int i = 0; + if (this->data[1][1] > this->data[0][0]) + i = 1; + if (this->data[2][2] > this->data[i][i]) + i = 2; + unsigned int j = s_iNext[i]; + unsigned int k = s_iNext[j]; + + root = sqrt(this->data[i][i] - this->data[j][j] - + this->data[k][k] + 1.0); + + T a, b, c; + a = root / 2.0; + root = 1.0 / (2.0 * root); + b = (this->data[j][i] + this->data[i][j]) * root; + c = (this->data[k][i] + this->data[i][k]) * root; + + switch (i) + { + default: + case 0: q.X(a); break; + case 1: q.Y(a); break; + case 2: q.Z(a); break; + }; + switch (j) + { + default: + case 0: q.X(b); break; + case 1: q.Y(b); break; + case 2: q.Z(b); break; + }; + switch (k) + { + default: + case 0: q.X(c); break; + case 1: q.Y(c); break; + case 2: q.Z(c); break; + }; + + q.W((this->data[k][j] - this->data[j][k]) * root); + } + + return q; + } + + /// \brief Get the rotation as a Euler angles + /// \param[in] _firstSolution True to get the first Euler solution, + /// false to get the second. + /// \return the rotation + public: Vector3 EulerRotation(bool _firstSolution) const + { + Vector3 euler; + Vector3 euler2; + + T m31 = this->data[2][0]; + T m11 = this->data[0][0]; + T m12 = this->data[0][1]; + T m13 = this->data[0][2]; + T m32 = this->data[2][1]; + T m33 = this->data[2][2]; + T m21 = this->data[1][0]; + + if (std::abs(m31) >= 1.0) + { + euler.Z(0.0); + euler2.Z(0.0); + + if (m31 < 0.0) + { + euler.Y(IGN_PI / 2.0); + euler2.Y(IGN_PI / 2.0); + euler.X(atan2(m12, m13)); + euler2.X(atan2(m12, m13)); + } + else + { + euler.Y(-IGN_PI / 2.0); + euler2.Y(-IGN_PI / 2.0); + euler.X(atan2(-m12, -m13)); + euler2.X(atan2(-m12, -m13)); + } + } + else + { + euler.Y(-asin(m31)); + euler2.Y(IGN_PI - euler.Y()); + + euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y()))); + euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y()))); + + euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y()))); + euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y()))); + } + + if (_firstSolution) + return euler; + else + return euler2; + } + + /// \brief Get the transformation as math::Pose + /// \return the pose + public: Pose3 Pose() const + { + return Pose3(this->Translation(), this->Rotation()); + } + + /// \brief Set the scale + /// \param[in] _s scale + public: void Scale(const Vector3 &_s) + { + this->data[0][0] = _s.X(); + this->data[1][1] = _s.Y(); + this->data[2][2] = _s.Z(); + this->data[3][3] = 1.0; + } + + /// \brief Set the scale + /// \param[in] _x X scale value. + /// \param[in] _y Y scale value. + /// \param[in] _z Z scale value. + public: void Scale(T _x, T _y, T _z) + { + this->data[0][0] = _x; + this->data[1][1] = _y; + this->data[2][2] = _z; + this->data[3][3] = 1.0; + } + + /// \brief Return true if the matrix is affine + /// \return true if the matrix is affine, false otherwise + public: bool IsAffine() const + { + return equal(this->data[3][0], static_cast(0)) && + equal(this->data[3][1], static_cast(0)) && + equal(this->data[3][2], static_cast(0)) && + equal(this->data[3][3], static_cast(1)); + } + + /// \brief Perform an affine transformation + /// \param[in] _v Vector3 value for the transformation + /// \return The result of the transformation. A default constructed + /// Vector3 is returned if this matrix is not affine. + /// \deprecated Use bool TransformAffine(const Vector3 &_v, + /// Vector3 &_result) const; + public: Vector3 + IGN_DEPRECATED(3.0) + TransformAffine(const Vector3 &_v) const + { + if (this->IsAffine()) + { + return Vector3(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + + this->data[0][2]*_v.Z() + this->data[0][3], + this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + + this->data[1][2]*_v.Z() + this->data[1][3], + this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + + this->data[2][2]*_v.Z() + this->data[2][3]); + } + else + { + return Vector3(); + } + } + + /// \brief Perform an affine transformation + /// \param[in] _v Vector3 value for the transformation + /// \param[out] _result The result of the transformation. _result is + /// not changed if this matrix is not affine. + /// \return True if this matrix is affine, false otherwise. + public: bool TransformAffine(const Vector3 &_v, + Vector3 &_result) const + { + if (!this->IsAffine()) + return false; + + _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + + this->data[0][2]*_v.Z() + this->data[0][3], + this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + + this->data[1][2]*_v.Z() + this->data[1][3], + this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + + this->data[2][2]*_v.Z() + this->data[2][3]); + return true; + } + + /// \brief Return the determinant of the matrix + /// \return Determinant of this matrix. + public: T Determinant() const + { + T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; + + v0 = this->data[2][0]*this->data[3][1] + - this->data[2][1]*this->data[3][0]; + v1 = this->data[2][0]*this->data[3][2] + - this->data[2][2]*this->data[3][0]; + v2 = this->data[2][0]*this->data[3][3] + - this->data[2][3]*this->data[3][0]; + v3 = this->data[2][1]*this->data[3][2] + - this->data[2][2]*this->data[3][1]; + v4 = this->data[2][1]*this->data[3][3] + - this->data[2][3]*this->data[3][1]; + v5 = this->data[2][2]*this->data[3][3] + - this->data[2][3]*this->data[3][2]; + + t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3]; + t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3]; + t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3]; + t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2]; + + return t00 * this->data[0][0] + + t10 * this->data[0][1] + + t20 * this->data[0][2] + + t30 * this->data[0][3]; + } + + /// \brief Return the inverse matrix. + /// This is a non-destructive operation. + /// \return Inverse of this matrix. + public: Matrix4 Inverse() const + { + T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; + Matrix4 r; + + v0 = this->data[2][0]*this->data[3][1] - + this->data[2][1]*this->data[3][0]; + v1 = this->data[2][0]*this->data[3][2] - + this->data[2][2]*this->data[3][0]; + v2 = this->data[2][0]*this->data[3][3] - + this->data[2][3]*this->data[3][0]; + v3 = this->data[2][1]*this->data[3][2] - + this->data[2][2]*this->data[3][1]; + v4 = this->data[2][1]*this->data[3][3] - + this->data[2][3]*this->data[3][1]; + v5 = this->data[2][2]*this->data[3][3] - + this->data[2][3]*this->data[3][2]; + + t00 = +(v5*this->data[1][1] - + v4*this->data[1][2] + v3*this->data[1][3]); + t10 = -(v5*this->data[1][0] - + v2*this->data[1][2] + v1*this->data[1][3]); + t20 = +(v4*this->data[1][0] - + v2*this->data[1][1] + v0*this->data[1][3]); + t30 = -(v3*this->data[1][0] - + v1*this->data[1][1] + v0*this->data[1][2]); + + T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] + + t20 * this->data[0][2] + t30 * this->data[0][3]); + + r(0, 0) = t00 * invDet; + r(1, 0) = t10 * invDet; + r(2, 0) = t20 * invDet; + r(3, 0) = t30 * invDet; + + r(0, 1) = -(v5*this->data[0][1] - + v4*this->data[0][2] + v3*this->data[0][3]) * invDet; + r(1, 1) = +(v5*this->data[0][0] - + v2*this->data[0][2] + v1*this->data[0][3]) * invDet; + r(2, 1) = -(v4*this->data[0][0] - + v2*this->data[0][1] + v0*this->data[0][3]) * invDet; + r(3, 1) = +(v3*this->data[0][0] - + v1*this->data[0][1] + v0*this->data[0][2]) * invDet; + + v0 = this->data[1][0]*this->data[3][1] - + this->data[1][1]*this->data[3][0]; + v1 = this->data[1][0]*this->data[3][2] - + this->data[1][2]*this->data[3][0]; + v2 = this->data[1][0]*this->data[3][3] - + this->data[1][3]*this->data[3][0]; + v3 = this->data[1][1]*this->data[3][2] - + this->data[1][2]*this->data[3][1]; + v4 = this->data[1][1]*this->data[3][3] - + this->data[1][3]*this->data[3][1]; + v5 = this->data[1][2]*this->data[3][3] - + this->data[1][3]*this->data[3][2]; + + r(0, 2) = +(v5*this->data[0][1] - + v4*this->data[0][2] + v3*this->data[0][3]) * invDet; + r(1, 2) = -(v5*this->data[0][0] - + v2*this->data[0][2] + v1*this->data[0][3]) * invDet; + r(2, 2) = +(v4*this->data[0][0] - + v2*this->data[0][1] + v0*this->data[0][3]) * invDet; + r(3, 2) = -(v3*this->data[0][0] - + v1*this->data[0][1] + v0*this->data[0][2]) * invDet; + + v0 = this->data[2][1]*this->data[1][0] - + this->data[2][0]*this->data[1][1]; + v1 = this->data[2][2]*this->data[1][0] - + this->data[2][0]*this->data[1][2]; + v2 = this->data[2][3]*this->data[1][0] - + this->data[2][0]*this->data[1][3]; + v3 = this->data[2][2]*this->data[1][1] - + this->data[2][1]*this->data[1][2]; + v4 = this->data[2][3]*this->data[1][1] - + this->data[2][1]*this->data[1][3]; + v5 = this->data[2][3]*this->data[1][2] - + this->data[2][2]*this->data[1][3]; + + r(0, 3) = -(v5*this->data[0][1] - + v4*this->data[0][2] + v3*this->data[0][3]) * invDet; + r(1, 3) = +(v5*this->data[0][0] - + v2*this->data[0][2] + v1*this->data[0][3]) * invDet; + r(2, 3) = -(v4*this->data[0][0] - + v2*this->data[0][1] + v0*this->data[0][3]) * invDet; + r(3, 3) = +(v3*this->data[0][0] - + v1*this->data[0][1] + v0*this->data[0][2]) * invDet; + + return r; + } + + /// \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[1][2], this->data[2][1]); + std::swap(this->data[1][3], this->data[3][1]); + std::swap(this->data[2][3], this->data[3][2]); + } + + /// \brief Return the transpose of this matrix + /// \return Transpose of this matrix. + public: Matrix4 Transposed() const + { + return Matrix4( + this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0], + this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1], + this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2], + this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]); + } + + /// \brief Equal operator. this = _mat + /// \param _mat Incoming matrix + /// \return itself + public: Matrix4 &operator=(const Matrix4 &_mat) + { + memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16); + return *this; + } + + /// \brief Equal operator for 3x3 matrix + /// \param _mat Incoming matrix + /// \return itself + public: const Matrix4 &operator=(const Matrix3 &_mat) + { + this->data[0][0] = _mat(0, 0); + this->data[0][1] = _mat(0, 1); + this->data[0][2] = _mat(0, 2); + + this->data[1][0] = _mat(1, 0); + this->data[1][1] = _mat(1, 1); + this->data[1][2] = _mat(1, 2); + + this->data[2][0] = _mat(2, 0); + this->data[2][1] = _mat(2, 1); + this->data[2][2] = _mat(2, 2); + + return *this; + } + + /// \brief Multiplication assignment operator. This matrix will + /// become equal to this * _m2. + /// \param[in] _m2 Incoming matrix. + /// \return This matrix * _mat. + public: Matrix4 operator*=(const Matrix4 &_m2) + { + (*this) = (*this) * _m2; + return *this; + } + + /// \brief Multiplication operator + /// \param[in] _m2 Incoming matrix + /// \return This matrix * _mat + public: Matrix4 operator*(const Matrix4 &_m2) const + { + return Matrix4( + 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][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][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][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[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][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][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][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[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][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][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][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[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][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][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][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)); + } + + /// \brief Multiplication operator + /// \param _vec Vector3 + /// \return Resulting vector from multiplication + public: Vector3 operator*(const Vector3 &_vec) const + { + return Vector3( + this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() + + this->data[0][2]*_vec.Z() + this->data[0][3], + this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() + + this->data[1][2]*_vec.Z() + this->data[1][3], + this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() + + this->data[2][2]*_vec.Z() + this->data[2][3]); + } + + /// \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_THREE_SIZE_T)][ + clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_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_THREE_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_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 Matrix4 &_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[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[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[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); + } + + /// \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 Matrix4 &_m) const + { + return this->Equal(_m, static_cast(1e-6)); + } + + /// \brief Inequality test operator + /// \param[in] _m Matrix4 to test + /// \return True if not equal (using the default tolerance of 1e-6) + public: bool operator!=(const Matrix4 &_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 gz::math::Matrix4 &_m) + { + _out << precision(_m(0, 0), 6) << " " + << precision(_m(0, 1), 6) << " " + << precision(_m(0, 2), 6) << " " + << precision(_m(0, 3), 6) << " " + << precision(_m(1, 0), 6) << " " + << precision(_m(1, 1), 6) << " " + << precision(_m(1, 2), 6) << " " + << precision(_m(1, 3), 6) << " " + << precision(_m(2, 0), 6) << " " + << precision(_m(2, 1), 6) << " " + << precision(_m(2, 2), 6) << " " + << precision(_m(2, 3), 6) << " " + << precision(_m(3, 0), 6) << " " + << precision(_m(3, 1), 6) << " " + << precision(_m(3, 2), 6) << " " + << precision(_m(3, 3), 6); + + return _out; + } + + /// \brief Stream extraction operator + /// \param[in,out] _in input stream + /// \param[out] _m Matrix4 to read values into + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, gz::math::Matrix4 &_m) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + T d[16]; + _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]; + + 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]); + } + return _in; + } + + /// \brief Get transform which translates to _eye and rotates the X axis + /// so it faces the _target. The rotation is such that Z axis is in the + /// _up direction, if possible. The coordinate system is right-handed, + /// \param[in] _eye Coordinate frame translation. + /// \param[in] _target Point which the X axis should face. If _target is + /// equal to _eye, the X axis won't be rotated. + /// \param[in] _up Direction in which the Z axis should point. If _up is + /// zero or parallel to X, it will be set to +Z. + /// \return Transformation matrix. + public: static Matrix4 LookAt(const Vector3 &_eye, + const Vector3 &_target, const Vector3 &_up = Vector3::UnitZ) + { + // Most important constraint: direction to point X axis at + auto front = _target - _eye; + + // Case when _eye == _target + if (front == Vector3::Zero) + front = Vector3::UnitX; + front.Normalize(); + + // Desired direction to point Z axis at + auto up = _up; + + // Case when _up == Zero + if (up == Vector3::Zero) + up = Vector3::UnitZ; + else + up.Normalize(); + + // Case when _up is parallel to X + if (up.Cross(Vector3::UnitX) == Vector3::Zero) + up = Vector3::UnitZ; + + // Find direction to point Y axis at + auto left = up.Cross(front); + + // Case when front is parallel to up + if (left == Vector3::Zero) + left = Vector3::UnitY; + else + left.Normalize(); + + // Fix up direction so it's perpendicular to XY + up = (front.Cross(left)).Normalize(); + + return Matrix4( + front.X(), left.X(), up.X(), _eye.X(), + front.Y(), left.Y(), up.Y(), _eye.Y(), + front.Z(), left.Z(), up.Z(), _eye.Z(), + 0, 0, 0, 1); + } + + /// \brief The 4x4 matrix + private: T data[4][4]; + }; + + template + const Matrix4 Matrix4::Identity( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); + + template + const Matrix4 Matrix4::Zero( + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0); + + typedef Matrix4 Matrix4i; + typedef Matrix4 Matrix4d; + typedef Matrix4 Matrix4f; + } + } +} +#endif diff --git a/include/gz/math/Matrix6.hh b/include/gz/math/Matrix6.hh new file mode 100644 index 000000000..3a793648c --- /dev/null +++ b/include/gz/math/Matrix6.hh @@ -0,0 +1,593 @@ +/* + * 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 +#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 Identifiers for each of the 4 3x3 corners of the matrix. + public: enum Matrix6Corner + { + /// \brief Top-left corner, consisting of the intersection between the + /// first 3 rows and first 3 columns. + TOP_LEFT = 0, + + /// \brief Top-right corner, consisting of the intersection between the + /// first 3 rows and last 3 columns. + TOP_RIGHT = 1, + + /// \brief Bottom-left corner, consisting of the intersection between + /// the last 3 rows and first 3 columns. + BOTTOM_LEFT = 2, + + /// \brief Bottom-right corner, consisting of the intersection between + /// the last 3 rows and last 3 columns. + BOTTOM_RIGHT = 3 + }; + + /// \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 Set a value in a specific row and col + /// param[in] _row Row of the matrix + /// param[in] _col Col of the matrix + /// param[in] _v Value to assign + /// \return Tru if the value was setted, False otherwise + public: bool SetValue(size_t _row, size_t _col, T _v) + { + if (_row < MatrixSize && _col < MatrixSize) + { + this->data[_row][_col] = _v; + return true; + } + return false; + } + + /// \brief Change the values + /// \param[in] _v00 Row 0, Col 0 value + /// \param[in] _v01 Row 0, Col 1 value + /// \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, + 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 * _m2. + public: Matrix6 operator*=(const Matrix6 &_m2) + { + (*this) = (*this) * _m2; + return *this; + } + + /// \brief Multiplication operator + /// \param[in] _m2 Incoming matrix + /// \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( + 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 Addition assignment operator. This matrix will + /// become equal to this + _m2. + /// \param[in] _m2 Incoming matrix. + /// \return This matrix + _m2. + public: Matrix6 operator+=(const Matrix6 &_m2) + { + (*this) = (*this) + _m2; + return *this; + } + + /// \brief Addition operator + /// \param[in] _m2 Incoming matrix + /// \return This matrix + _m2 + public: Matrix6 operator+(const Matrix6 &_m2) const + { + auto el = [&](size_t _row, size_t _col) -> T + { + return this->data[_row][_col] + _m2(_row, _col); + }; + return Matrix6( + el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5), + el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5), + el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5), + el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5), + el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5), + el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5)); + } + + /// \brief Get the value at the specified row, column index + /// \param[in] _col The column index. Index values are clamped to a + /// range of [0, 5]. + /// \param[in] _row the row index. Index values are clamped to a + /// 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 + { + 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 of the value at the specified row, + /// column index + /// \param[in] _row the row index. Index values are clamped to a + /// range of [0, 5]. + /// \param[in] _col The column index. Index values are clamped to a + /// range of [0, 5]. + /// \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 that + /// 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(Matrix6Corner _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 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] _mat 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. + /// \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 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 + { + 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 gz::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, gz::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/include/gz/math/MovingWindowFilter.hh b/include/gz/math/MovingWindowFilter.hh new file mode 100644 index 000000000..a0d6be5f3 --- /dev/null +++ b/include/gz/math/MovingWindowFilter.hh @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2016 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_MOVINGWINDOWFILTER_HH_ +#define GZ_MATH_MOVINGWINDOWFILTER_HH_ + +#include +#include +#include "gz/math/Export.hh" + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + + /// \cond + /// \brief Private data members for MovingWindowFilter class. + /// This must be in the header due to templatization. + template< typename T> + class MovingWindowFilterPrivate + { + // \brief Constructor + public: MovingWindowFilterPrivate(); + + /// \brief For moving window smoothed value + public: unsigned int valWindowSize = 4; + + /// \brief buffer history of raw values + public: std::vector valHistory; + + /// \brief iterator pointing to current value in buffer + public: typename std::vector::iterator valIter; + + /// \brief keep track of running sum + public: T sum; + + /// \brief keep track of number of elements + public: unsigned int samples = 0; + }; + + ////////////////////////////////////////////////// + template + MovingWindowFilterPrivate::MovingWindowFilterPrivate() + { + /// \TODO FIXME hardcoded initial value for now + this->valHistory.resize(this->valWindowSize); + this->valIter = this->valHistory.begin(); + this->sum = T(); + } + /// \endcond + + /// \brief Base class for MovingWindowFilter. This replaces the + /// version of MovingWindowFilter in the Ignition Common library. + /// + /// The default window size is 4. + template< typename T> + class MovingWindowFilter + { + /// \brief Constructor + public: MovingWindowFilter(); + + /// \brief Destructor + public: virtual ~MovingWindowFilter(); + + /// \brief Update value of filter + /// \param[in] _val new raw value + public: void Update(const T _val); + + /// \brief Set window size + /// \param[in] _n new desired window size + public: void SetWindowSize(const unsigned int _n); + + /// \brief Get the window size. + /// \return The size of the moving window. + public: unsigned int WindowSize() const; + + /// \brief Get whether the window has been filled. + /// \return True if the window has been filled. + public: bool WindowFilled() const; + + /// \brief Get filtered result + /// \return Latest filtered value + public: T Value() const; + + /// \brief Data pointer. + private: std::unique_ptr> dataPtr; + }; + + ////////////////////////////////////////////////// + template + MovingWindowFilter::MovingWindowFilter() + : dataPtr(new MovingWindowFilterPrivate()) + { + } + + ////////////////////////////////////////////////// + template + MovingWindowFilter::~MovingWindowFilter() + { + this->dataPtr->valHistory.clear(); + } + + ////////////////////////////////////////////////// + template + void MovingWindowFilter::Update(const T _val) + { + // update sum and sample size with incoming _val + + // keep running sum + this->dataPtr->sum += _val; + + // shift pointer, wrap around if end has been reached. + ++this->dataPtr->valIter; + if (this->dataPtr->valIter == this->dataPtr->valHistory.end()) + { + // reset iterator to beginning of queue + this->dataPtr->valIter = this->dataPtr->valHistory.begin(); + } + + // increment sample size + ++this->dataPtr->samples; + + if (this->dataPtr->samples > this->dataPtr->valWindowSize) + { + // subtract old value if buffer already filled + this->dataPtr->sum -= (*this->dataPtr->valIter); + // put new value into queue + (*this->dataPtr->valIter) = _val; + // reduce sample size + --this->dataPtr->samples; + } + else + { + // put new value into queue + (*this->dataPtr->valIter) = _val; + } + } + + ////////////////////////////////////////////////// + template + void MovingWindowFilter::SetWindowSize(const unsigned int _n) + { + this->dataPtr->valWindowSize = _n; + this->dataPtr->valHistory.clear(); + this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize); + this->dataPtr->valIter = this->dataPtr->valHistory.begin(); + this->dataPtr->sum = T(); + this->dataPtr->samples = 0; + } + + ////////////////////////////////////////////////// + template + unsigned int MovingWindowFilter::WindowSize() const + { + return this->dataPtr->valWindowSize; + } + + ////////////////////////////////////////////////// + template + bool MovingWindowFilter::WindowFilled() const + { + return this->dataPtr->samples == this->dataPtr->valWindowSize; + } + + ////////////////////////////////////////////////// + template + T MovingWindowFilter::Value() const + { + return this->dataPtr->sum / static_cast(this->dataPtr->samples); + } + } + } +} +#endif diff --git a/include/gz/math/OrientedBox.hh b/include/gz/math/OrientedBox.hh new file mode 100644 index 000000000..b81cb682a --- /dev/null +++ b/include/gz/math/OrientedBox.hh @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2017 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_ORIENTEDBOX_HH_ +#define GZ_MATH_ORIENTEDBOX_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \brief Mathematical representation of a box which can be arbitrarily + /// positioned and rotated. + template + class OrientedBox + { + /// \brief Default constructor + public: OrientedBox() : size(Vector3::Zero), pose(Pose3::Zero) + { + } + + /// \brief Constructor which takes size and pose. + /// \param[in] _size Box size, in its own coordinate frame. Its absolute + /// value will be taken, so the size is non-negative. + /// \param[in] _pose Box pose. + public: OrientedBox(const Vector3 &_size, const Pose3 &_pose) + : size(_size.Abs()), pose(_pose) + { + } + + /// \brief Constructor which takes size, pose, and material. + /// \param[in] _size Box size, in its own coordinate frame. Its absolute + /// value will be taken, so the size is non-negative. + /// \param[in] _pose Box pose. + /// \param[in] _mat Material property for the box. + public: OrientedBox(const Vector3 &_size, const Pose3 &_pose, + const Material &_mat) + : size(_size.Abs()), pose(_pose), material(_mat) + { + } + + /// \brief Constructor which takes only the size. + /// \param[in] _size Box size, in its own coordinate frame. Its absolute + /// value will be taken, so the size is non-negative. + public: explicit OrientedBox(const Vector3 &_size) + : size(_size.Abs()), pose(Pose3::Zero) + { + } + + /// \brief Constructor which takes only the size. + /// \param[in] _size Box size, in its own coordinate frame. Its absolute + /// value will be taken, so the size is non-negative. + /// \param[in] _mat Material property for the box. + public: explicit OrientedBox(const Vector3 &_size, + const Material &_mat) + : size(_size.Abs()), pose(Pose3::Zero), material(_mat) + { + } + + /// \brief Copy constructor. + /// \param[in] _b OrientedBox to copy. + public: OrientedBox(const OrientedBox &_b) + : size(_b.size), pose(_b.pose), material(_b.material) + { + } + + /// \brief Destructor + public: virtual ~OrientedBox() + { + } + + /// \brief Get the length along the x dimension + /// \return Value of the length in the x dimension + public: T XLength() const + { + return this->size.X(); + } + + /// \brief Get the length along the y dimension + /// \return Value of the length in the y dimension + public: T YLength() const + { + return this->size.Y(); + } + + /// \brief Get the length along the z dimension + /// \return Value of the length in the z dimension + public: T ZLength() const + { + return this->size.Z(); + } + + /// \brief Get the size of the box + /// \return Size of the box + public: const Vector3 &Size() const + { + return this->size; + } + + /// \brief Get the box pose, which is the pose of its center. + /// \return The pose of the box. + public: const Pose3 &Pose() const + { + return this->pose; + } + + /// \brief Set the box size. + /// \param[in] _size Box size, in its own coordinate frame. Its absolute + /// value will be taken, so the size is non-negative. + public: void Size(Vector3 &_size) + { + // Enforce non-negative size + this->size = _size.Abs(); + } + + /// \brief Set the box pose. + /// \param[in] _pose Box pose. + public: void Pose(Pose3 &_pose) + { + this->pose = _pose; + } + + /// \brief Assignment operator. Set this box to the parameter + /// \param[in] _b OrientedBox to copy + /// \return The new box. + public: OrientedBox &operator=(const OrientedBox &_b) + { + this->size = _b.size; + this->pose = _b.pose; + this->material = _b.material; + return *this; + } + + /// \brief Equality test operator + /// \param[in] _b OrientedBox to test + /// \return True if equal + public: bool operator==(const OrientedBox &_b) const + { + return this->size == _b.size && this->pose == _b.pose && + this->material == _b.material; + } + + /// \brief Inequality test operator + /// \param[in] _b OrientedBox to test + /// \return True if not equal + public: bool operator!=(const OrientedBox &_b) const + { + return this->size != _b.size || this->pose != _b.pose || + this->material != _b.material; + } + + /// \brief Output operator + /// \param[in] _out Output stream + /// \param[in] _b OrientedBox to output to the stream + /// \return The stream + public: friend std::ostream &operator<<(std::ostream &_out, + const OrientedBox &_b) + { + _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] " + << "Material[" << _b.Material().Name() << "]"; + return _out; + } + + /// \brief Check if a point lies inside the box. + /// \param[in] _p Point to check. + /// \return True if the point is inside the box. + public: bool Contains(const Vector3d &_p) const + { + // Move point to box frame + auto t = Matrix4(this->pose).Inverse(); + auto p = t *_p; + + return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 && + p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 && + p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5; + } + + /// \brief Get the material associated with this box. + /// \return The material assigned to this box. + public: const gz::math::Material &Material() const + { + return this->material; + } + + /// \brief Set the material associated with this box. + /// \param[in] _mat The material assigned to this box. + public: void SetMaterial(const gz::math::Material &_mat) + { + this->material = _mat; + } + + /// \brief Get the volume of the box in m^3. + /// \return Volume of the box in m^3. + public: T Volume() const + { + return this->size.X() * this->size.Y() * this->size.Z(); + } + + /// \brief Compute the box's density given a mass value. The + /// box is assumed to be solid with uniform density. This + /// function requires the box's size to be set to + /// values greater than zero. The Material of the box is ignored. + /// \param[in] _mass Mass of the box, in kg. This value should be + /// greater than zero. + /// \return Density of the box in kg/m^3. A negative value is + /// returned if the size or _mass is <= 0. + public: T DensityFromMass(const T _mass) const + { + if (this->size.Min() <= 0|| _mass <= 0) + return -1.0; + + return _mass / this->Volume(); + } + + /// \brief Set the density of this box based on a mass value. + /// Density is computed using + /// double DensityFromMass(const double _mass) const. The + /// box is assumed to be solid with uniform density. This + /// function requires the box's size to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the box, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// box's size or the _mass value are <= 0. + /// \sa double DensityFromMass(const double _mass) const + public: bool SetDensityFromMass(const T _mass) + { + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; + } + + /// \brief Get the mass matrix for this box. This function + /// is only meaningful if the box's size and material + /// have been set. + /// \param[out] _massMat The computed mass matrix will be stored here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid size (<=0) or density (<=0). + public: bool MassMatrix(MassMatrix3 &_massMat) const + { + return _massMat.SetFromBox(this->material, this->size); + } + + /// \brief The size of the box in its local frame. + private: Vector3 size; + + /// \brief The pose of the center of the box. + private: Pose3 pose; + + /// \brief The box's material. + private: gz::math::Material material; + }; + + typedef OrientedBox OrientedBoxi; + typedef OrientedBox OrientedBoxd; + typedef OrientedBox OrientedBoxf; + } + } +} +#endif diff --git a/include/gz/math/PID.hh b/include/gz/math/PID.hh new file mode 100644 index 000000000..bf40356c8 --- /dev/null +++ b/include/gz/math/PID.hh @@ -0,0 +1,234 @@ +/* + * Copyright (C) 2016 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_PID_HH_ +#define GZ_MATH_PID_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class PID PID.hh ignition/math/PID.hh + /// \brief Generic PID controller class. + /// Generic proportional-integral-derivative controller class that + /// keeps track of PID-error states and control inputs given + /// the state of a system and a user specified target state. + /// It includes a user-adjustable command offset term (feed-forward). + // cppcheck-suppress class_X_Y + class IGNITION_MATH_VISIBLE PID + { + /// \brief Constructor, zeros out Pid values when created and + /// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2]. + /// + /// Disable command clamping by setting _cmdMin to a value larger + /// than _cmdMax. Command clamping is disabled by default. + /// + /// Disable integral clamping by setting _iMin to a value larger + /// than _iMax. Integral clamping is disabled by default. + /// + /// \param[in] _p The proportional gain. + /// \param[in] _i The integral gain. + /// \param[in] _d The derivative gain. + /// \param[in] _imax The integral upper limit. + /// \param[in] _imin The integral lower limit. + /// \param[in] _cmdMax Output max value. + /// \param[in] _cmdMin Output min value. + /// \param[in] _cmdOffset Command offset (feed-forward). + public: PID(const double _p = 0.0, + const double _i = 0.0, + const double _d = 0.0, + const double _imax = -1.0, + const double _imin = 0.0, + const double _cmdMax = -1.0, + const double _cmdMin = 0.0, + const double _cmdOffset = 0.0); + + /// \brief Destructor + public: ~PID() = default; + + /// \brief Initialize PID-gains and integral term + /// limits:[iMax:iMin]-[I1:I2]. + /// + /// Disable command clamping by setting _cmdMin to a value larger + /// than _cmdMax. Command clamping is disabled by default. + /// + /// Disable integral clamping by setting _iMin to a value larger + /// than _iMax. Integral clamping is disabled by default. + /// + /// \param[in] _p The proportional gain. + /// \param[in] _i The integral gain. + /// \param[in] _d The derivative gain. + /// \param[in] _imax The integral upper limit. + /// \param[in] _imin The integral lower limit. + /// \param[in] _cmdMax Output max value. + /// \param[in] _cmdMin Output min value. + /// \param[in] _cmdOffset Command offset (feed-forward). + public: void Init(const double _p = 0.0, + const double _i = 0.0, + const double _d = 0.0, + const double _imax = -1.0, + const double _imin = 0.0, + const double _cmdMax = -1.0, + const double _cmdMin = 0.0, + const double _cmdOffset = 0.0); + + /// \brief Set the proportional Gain. + /// \param[in] _p proportional gain value + public: void SetPGain(const double _p); + + /// \brief Set the integral Gain. + /// \param[in] _i integral gain value + public: void SetIGain(const double _i); + + /// \brief Set the derivative Gain. + /// \param[in] _d derivative gain value + public: void SetDGain(const double _d); + + /// \brief Set the integral upper limit. + /// \param[in] _i integral upper limit value + public: void SetIMax(const double _i); + + /// \brief Set the integral lower limit. + /// \param[in] _i integral lower limit value + public: void SetIMin(const double _i); + + /// \brief Set the maximum value for the command. + /// \param[in] _c The maximum value + public: void SetCmdMax(const double _c); + + /// \brief Set the minimum value for the command. + /// \param[in] _c The minimum value + public: void SetCmdMin(const double _c); + + /// \brief Set the offset value for the command, + /// which is added to the result of the PID controller. + /// \param[in] _c The offset value + public: void SetCmdOffset(const double _c); + + /// \brief Get the proportional Gain. + /// \return The proportional gain value + public: double PGain() const; + + /// \brief Get the integral Gain. + /// \return The integral gain value + public: double IGain() const; + + /// \brief Get the derivative Gain. + /// \return The derivative gain value + public: double DGain() const; + + /// \brief Get the integral upper limit. + /// \return The integral upper limit value + public: double IMax() const; + + /// \brief Get the integral lower limit. + /// \return The integral lower limit value + public: double IMin() const; + + /// \brief Get the maximum value for the command. + /// \return The maximum value + public: double CmdMax() const; + + /// \brief Get the minimun value for the command. + /// \return The maximum value + public: double CmdMin() const; + + /// \brief Get the offset value for the command. + /// \return The offset value + public: double CmdOffset() const; + + /// \brief Update the Pid loop with nonuniform time step size. + /// \param[in] _error Error since last call (p_state - p_target). + /// \param[in] _dt Change in time since last update call. + /// Normally, this is called at every time step, + /// The return value is an updated command to be passed + /// to the object being controlled. + /// \return the command value + public: double Update(const double _error, + const std::chrono::duration &_dt); + + /// \brief Set current target command for this PID controller. + /// \param[in] _cmd New command + public: void SetCmd(const double _cmd); + + /// \brief Return current command for this PID controller. + /// \return the command value + public: double Cmd() const; + + /// \brief Return PID error terms for the controller. + /// \param[in] _pe The proportional error. + /// \param[in] _ie The integral of gain times error. + /// \param[in] _de The derivative error. + public: void Errors(double &_pe, double &_ie, double &_de) const; + + /// \brief Assignment operator + /// \param[in] _p a reference to a PID to assign values from + /// \return reference to this instance + public: PID &operator=(const PID &_p); + + /// \brief Reset the errors and command. + public: void Reset(); + + /// \brief Error at a previous step. + private: double pErrLast = 0.0; + + /// \brief Current error. + private: double pErr = 0.0; + + /// \brief Integral of gain times error. + private: double iErr = 0.0; + + /// \brief Derivative error. + private: double dErr = 0.0; + + /// \brief Gain for proportional control. + private: double pGain; + + /// \brief Gain for integral control. + private: double iGain = 0.0; + + /// \brief Gain for derivative control. + private: double dGain = 0.0; + + /// \brief Maximum clamping value for integral term. + private: double iMax = -1.0; + + /// \brief Minim clamping value for integral term. + private: double iMin = 0.0; + + /// \brief Command value. + private: double cmd = 0.0; + + /// \brief Max command clamping value. + private: double cmdMax = -1.0; + + /// \brief Min command clamping value. + private: double cmdMin = 0.0; + + /// \brief Command offset. + private: double cmdOffset = 0.0; + }; + } + } +} +#endif diff --git a/include/gz/math/PiecewiseScalarField3.hh b/include/gz/math/PiecewiseScalarField3.hh new file mode 100644 index 000000000..fefce8e51 --- /dev/null +++ b/include/gz/math/PiecewiseScalarField3.hh @@ -0,0 +1,219 @@ +/* + * 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_PIECEWISE_SCALAR_FIELD3_HH_ +#define GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /** \class PiecewiseScalarField3 PiecewiseScalarField3.hh\ + * ignition/math/PiecewiseScalarField3.hh + */ + /// \brief The PiecewiseScalarField3 class constructs a scalar field F + /// in R^3 as a union of scalar fields Pn, defined over regions Rn i.e. + /// piecewise. + /// + /// \tparam ScalarField3T a callable type taking a single Vector3 + /// value as argument and returning a ScalarT value. Additionally: + /// - for PiecewiseScalarField3 to have a stream operator overload, + /// ScalarField3T must support stream operator overload; + /// - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T + /// must implement a + /// ScalarT Minimum(const Region3 &, Vector3 &) + /// method that computes its minimum in the given region and returns + /// an argument value that yields said minimum. + /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits + /// have been specialized. + /// + /// ## Example + /// + /// \snippet examples/piecewise_scalar_field3_example.cc complete + template + class PiecewiseScalarField3 + { + /// \brief A scalar field P in R^3 and + /// the region R in which it is defined + public: struct Piece { + Region3 region; + ScalarField3T field; + }; + + /// \brief Constructor + public: PiecewiseScalarField3() = default; + + /// \brief Constructor + /// \param[in] _pieces scalar fields Pn and the regions Rn + /// in which these are defined. Regions should not overlap. + public: explicit PiecewiseScalarField3(const std::vector &_pieces) + : pieces(_pieces) + { + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i].region.Empty()) + { + std::cerr << "Region #" << i << " (" << pieces[i].region + << ") in piecewise scalar field definition is empty." + << std::endl; + } + for (size_t j = i + 1; j < pieces.size(); ++j) + { + if (pieces[i].region.Intersects(pieces[j].region)) + { + std::cerr << "Detected overlap between regions in " + << "piecewise scalar field definition: " + << "region #" << i << " (" << pieces[i].region + << ") overlaps with region #" << j << " (" + << pieces[j].region << "). Region #" << i + << " will take precedence when overlapping." + << std::endl; + } + } + } + } + + /// \brief Define piecewise scalar field as `_field` throughout R^3 space + /// \param[in] _field a scalar field in R^3 + /// \return `_field` as a piecewise scalar field + public: static PiecewiseScalarField3 Throughout(ScalarField3T _field) + { + return PiecewiseScalarField3({ + {Region3::Unbounded, std::move(_field)}}); + } + + /// \brief Evaluate the piecewise scalar field at `_p` + /// \param[in] _p piecewise scalar field argument + /// \return the result of evaluating `F(_p)`, or NaN + /// if the scalar field is not defined at `_p` + public: ScalarT Evaluate(const Vector3 &_p) const + { + auto it = std::find_if( + this->pieces.begin(), this->pieces.end(), + [&](const Piece &piece) + { + return piece.region.Contains(_p); + }); + if (it == this->pieces.end()) + { + return std::numeric_limits::quiet_NaN(); + } + return it->field(_p); + } + + /// \brief Call operator overload + /// \see PiecewiseScalarField3::Evaluate() + /// \param[in] _p piecewise scalar field argument + /// \return the result of evaluating `F(_p)`, or NaN + /// if the scalar field is not defined at `_p` + public: ScalarT operator()(const Vector3 &_p) const + { + return this->Evaluate(_p); + } + + /// \brief Compute the piecewise scalar field minimum + /// Note that, since this method computes the minimum + /// for each region independently, it implicitly assumes + /// continuity in the boundaries between regions, if any. + /// \param[out] _pMin scalar field argument that yields + /// the minimum, or NaN if the scalar field is not + /// defined anywhere (i.e. default constructed) + /// \return the scalar field minimum, or NaN if the + /// scalar field is not defined anywhere (i.e. default + /// constructed) + public: ScalarT Minimum(Vector3 &_pMin) const + { + if (this->pieces.empty()) + { + _pMin = Vector3::NaN; + return std::numeric_limits::quiet_NaN(); + } + ScalarT yMin = std::numeric_limits::infinity(); + for (const Piece &piece : this->pieces) + { + if (!piece.region.Empty()) + { + Vector3 p; + const ScalarT y = piece.field.Minimum(piece.region, p); + if (y < yMin) + { + _pMin = p; + yMin = y; + } + } + } + return yMin; + } + + /// \brief Compute the piecewise scalar field minimum + /// \return the scalar field minimum, or NaN if the + /// scalar field is not defined anywhere (i.e. default + /// constructed) + public: ScalarT Minimum() const + { + Vector3 pMin; + return this->Minimum(pMin); + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _field SeparableScalarField3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, + const gz::math::PiecewiseScalarField3< + ScalarField3T, ScalarT> &_field) + { + if (_field.pieces.empty()) + { + return _out << "undefined"; + } + for (size_t i = 0; i < _field.pieces.size() - 1; ++i) + { + _out << _field.pieces[i].field << " if (x, y, z) in " + << _field.pieces[i].region << "; "; + } + return _out << _field.pieces.back().field + << " if (x, y, z) in " + << _field.pieces.back().region; + } + + /// \brief Scalar fields Pn and the regions Rn in which these are defined + private: std::vector pieces; + }; + + template + using PiecewiseScalarField3f = PiecewiseScalarField3; + template + using PiecewiseScalarField3d = PiecewiseScalarField3; + } + } +} + +#endif diff --git a/include/gz/math/Plane.hh b/include/gz/math/Plane.hh new file mode 100644 index 000000000..07dfbe66d --- /dev/null +++ b/include/gz/math/Plane.hh @@ -0,0 +1,291 @@ +/* + * Copyright (C) 2012 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_PLANE_HH_ +#define GZ_MATH_PLANE_HH_ + +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Plane Plane.hh ignition/math/Plane.hh + /// \brief A plane and related functions. + template + class Plane + { + /// \brief Enum used to indicate a side of the plane, no side, or both + /// sides for entities on the plane. + /// \sa Side + public: enum PlaneSide + { + /// \brief Negative side of the plane. This is the side that is + /// opposite the normal. + NEGATIVE_SIDE = 0, + + /// \brief Positive side of the plane. This is the side that has the + /// normal vector. + POSITIVE_SIDE = 1, + + /// \brief On the plane. + NO_SIDE = 2, + + /// \brief On both sides of the plane. + BOTH_SIDE = 3 + }; + + /// \brief Constructor + public: Plane() + : d(0.0) + { + } + + /// \brief Constructor from a normal and a distance + /// \param[in] _normal The plane normal + /// \param[in] _offset Offset along the normal + public: Plane(const Vector3 &_normal, T _offset = 0.0) + : normal(_normal), d(_offset) + { + } + + /// \brief Constructor + /// \param[in] _normal The plane normal + /// \param[in] _size Size of the plane + /// \param[in] _offset Offset along the normal + public: Plane(const Vector3 &_normal, const Vector2 &_size, + T _offset) + { + this->Set(_normal, _size, _offset); + } + + /// \brief Copy constructor + /// \param[in] _plane Plane to copy + public: Plane(const Plane &_plane) + : normal(_plane.normal), size(_plane.size), d(_plane.d) + {} + + /// \brief Destructor + public: virtual ~Plane() {} + + /// \brief Set the plane + /// \param[in] _normal The plane normal + /// \param[in] _offset Offset along the normal + public: void Set(const Vector3 &_normal, T _offset) + { + this->normal = _normal; + this->d = _offset; + } + + /// \brief Set the plane + /// \param[in] _normal The plane normal + /// \param[in] _size Size of the plane + /// \param[in] _offset Offset along the normal + public: void Set(const Vector3 &_normal, const Vector2 &_size, + T _offset) + { + this->normal = _normal; + this->size = _size; + this->d = _offset; + } + + /// \brief The distance to the plane from the given point. The + /// distance can be negative, which indicates the point is on the + /// negative side of the plane. + /// \param[in] _point 3D point to calculate distance from. + /// \return Distance from the point to the plane. + /// \sa Side + public: T Distance(const Vector3 &_point) const + { + return this->normal.Dot(_point) - this->d; + } + + /// \brief Get the intersection of an infinite line with the plane, + /// given the line's gradient and a point in parametrized space. + /// \param[in] _point A point that lies on the line. + /// \param[in] _gradient The gradient of the line. + /// \param[in] _tolerance The tolerance for determining a line is + /// parallel to the plane. Optional, default=10^-16 + /// \return The point of intersection. std::nullopt if the line is + /// parallel to the plane (including lines on the plane). + public: std::optional> Intersection( + const Vector3 &_point, + const Vector3 &_gradient, + const double &_tolerance = 1e-6) const + { + if (std::abs(this->Normal().Dot(_gradient)) < _tolerance) + { + return std::nullopt; + } + auto constant = this->Offset() - this->Normal().Dot(_point); + auto param = constant / this->Normal().Dot(_gradient); + auto intersection = _point + _gradient*param; + + if (this->Size() == Vector2(0, 0)) + return intersection; + + // Check if the point is within the size bounds + // To do this we create a Quaternion using Angle, Axis constructor and + // rotate the Y and X axis the same amount as the normal. + auto dotProduct = Vector3::UnitZ.Dot(this->Normal()); + auto angle = acos(dotProduct / this->Normal().Length()); + auto axis = Vector3::UnitZ.Cross(this->Normal().Normalized()); + Quaternion rotation(axis, angle); + + Vector3 rotatedXAxis = rotation * Vector3::UnitX; + Vector3 rotatedYAxis = rotation * Vector3::UnitY; + + auto xBasis = rotatedXAxis.Dot(intersection); + auto yBasis = rotatedYAxis.Dot(intersection); + + if (std::abs(xBasis) < this->Size().X() / 2 && + std::abs(yBasis) < this->Size().Y() / 2) + { + return intersection; + } + return std::nullopt; + } + + /// \brief The side of the plane a point is on. + /// \param[in] _point The 3D point to check. + /// \return Plane::NEGATIVE_SIDE if the distance from the point to the + /// plane is negative, Plane::POSITIVE_SIDE if the distance from the + /// point to the plane is positive, or Plane::NO_SIDE if the + /// point is on the plane. + public: PlaneSide Side(const Vector3 &_point) const + { + T dist = this->Distance(_point); + + if (dist < 0.0) + return NEGATIVE_SIDE; + + if (dist > 0.0) + return POSITIVE_SIDE; + + return NO_SIDE; + } + + /// \brief The side of the plane a box is on. + /// \param[in] _box The 3D box to check. + /// \return Plane::NEGATIVE_SIDE if the distance from the box to the + /// plane is negative, Plane::POSITIVE_SIDE if the distance from the + /// box to the plane is positive, or Plane::BOTH_SIDE if the + /// box is on the plane. + public: PlaneSide Side(const math::AxisAlignedBox &_box) const + { + double dist = this->Distance(_box.Center()); + double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0); + + if (dist < -maxAbsDist) + return NEGATIVE_SIDE; + + if (dist > maxAbsDist) + return POSITIVE_SIDE; + + return BOTH_SIDE; + } + + /// \brief Get distance to the plane give an origin and direction + /// \param[in] _origin the origin + /// \param[in] _dir a direction + /// \return the shortest distance + public: T Distance(const Vector3 &_origin, + const Vector3 &_dir) const + { + T denom = this->normal.Dot(_dir); + + if (std::abs(denom) < 1e-3) + { + // parallel + return 0; + } + else + { + T nom = _origin.Dot(this->normal) - this->d; + T t = -(nom/denom); + return t; + } + } + + /// \brief Get the plane size + public: inline const Vector2 &Size() const + { + return this->size; + } + + /// \brief Get the plane size + public: inline Vector2 &Size() + { + return this->size; + } + + /// \brief Get the plane offset + public: inline const Vector3 &Normal() const + { + return this->normal; + } + + /// \brief Get the plane offset + public: inline Vector3 &Normal() + { + return this->normal; + } + + /// \brief Get the plane offset + public: inline T Offset() const + { + return this->d; + } + + /// \brief Equal operator + /// \param _p another plane + /// \return itself + public: Plane &operator=(const Plane &_p) + { + this->normal = _p.normal; + this->size = _p.size; + this->d = _p.d; + + return *this; + } + + /// \brief Plane normal + private: Vector3 normal; + + /// \brief Plane size + private: Vector2 size; + + /// \brief Plane offset + private: T d; + }; + + typedef Plane Planei; + typedef Plane Planed; + typedef Plane Planef; + } + } +} + +#endif diff --git a/include/gz/math/Polynomial3.hh b/include/gz/math/Polynomial3.hh new file mode 100644 index 000000000..53148a387 --- /dev/null +++ b/include/gz/math/Polynomial3.hh @@ -0,0 +1,295 @@ +/* + * 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_POLYNOMIAL3_HH_ +#define GZ_MATH_POLYNOMIAL3_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Polynomial3 Polynomial3.hh ignition/math/Polynomial3.hh + /// \brief The Polynomial3 class represents a cubic polynomial + /// with real coefficients p(x) = c0 x^3 + c1 x^2 + c2 x + c3. + /// ## Example + /// + /// \snippet examples/polynomial3_example.cc complete + template + class Polynomial3 + { + /// \brief Constructor + public: Polynomial3() = default; + + /// \brief Constructor + /// \param[in] _coeffs coefficients c0 through c3, left to right + public: explicit Polynomial3(Vector4 _coeffs) + : coeffs(std::move(_coeffs)) + { + } + + /// \brief Make a constant polynomial + /// \return a p(x) = `_value` polynomial + public: static Polynomial3 Constant(T _value) + { + return Polynomial3(Vector4(0., 0., 0., _value)); + } + + /// \brief Get the polynomial coefficients + /// \return this polynomial coefficients + public: const Vector4 &Coeffs() const { return this->coeffs; } + + /// \brief Evaluate the polynomial at `_x` + /// For non-finite `_x`, this function + /// computes p(z) as z tends to `_x`. + /// \param[in] _x polynomial argument + /// \return the result of evaluating p(`_x`) + public: T Evaluate(const T &_x) const + { + using std::isnan, std::isfinite; + if (isnan(_x)) + { + return _x; + } + if (!isfinite(_x)) + { + using std::abs, std::copysign; + const T epsilon = + std::numeric_limits::epsilon(); + if (abs(this->coeffs[0]) >= epsilon) + { + return _x * copysign(T(1.), this->coeffs[0]); + } + if (abs(this->coeffs[1]) >= epsilon) + { + return copysign(_x, this->coeffs[1]); + } + if (abs(this->coeffs[2]) >= epsilon) + { + return _x * copysign(T(1.), this->coeffs[2]); + } + return this->coeffs[3]; + } + const T _x2 = _x * _x; + const T _x3 = _x2 * _x; + + return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 + + this->coeffs[2] * _x + this->coeffs[3]); + } + + /// \brief Call operator overload + /// \see Polynomial3::Evaluate() + public: T operator()(const T &_x) const + { + return this->Evaluate(_x); + } + + /// \brief Compute polynomial minimum in an `_interval` + /// \param[in] _interval polynomial argument interval to check + /// \param[out] _xMin polynomial argument that yields minimum, + /// or NaN if the interval is empty + /// \return the polynomial minimum in the given interval, + /// or NaN if the interval is empty + public: T Minimum(const Interval &_interval, T &_xMin) const + { + if (_interval.Empty()) + { + _xMin = std::numeric_limits::quiet_NaN(); + return std::numeric_limits::quiet_NaN(); + } + T yMin; + // For open intervals, assume continuity in the limit + const T &xLeft = _interval.LeftValue(); + const T &xRight = _interval.RightValue(); + const T yLeft = this->Evaluate(xLeft); + const T yRight = this->Evaluate(xRight); + if (yLeft <= yRight) + { + yMin = yLeft; + _xMin = xLeft; + } + else + { + yMin = yRight; + _xMin = xRight; + } + using std::abs, std::sqrt; // enable ADL + constexpr T epsilon = std::numeric_limits::epsilon(); + if (abs(this->coeffs[0]) >= epsilon) + { + // Polynomial function p(x) is cubic, look + // for local minima within the given interval + + // Find local extrema by computing the roots + // of p'(x), a quadratic polynomial function + const T a = this->coeffs[0] * T(3.); + const T b = this->coeffs[1] * T(2.); + const T c = this->coeffs[2]; + + const T discriminant = b * b - T(4.) * a * c; + if (discriminant >= T(0.)) + { + // Roots of p'(x) are real, check local minima + const T x = (-b + sqrt(discriminant)) / (T(2.) * a); + if (_interval.Contains(x)) + { + const T y = this->Evaluate(x); + if (y < yMin) + { + _xMin = x; + yMin = y; + } + } + } + } + else if (abs(this->coeffs[1]) >= epsilon) + { + // Polynomial function p(x) is quadratic, + // look for global minima if concave + const T a = this->coeffs[1]; + const T b = this->coeffs[2]; + if (a > T(0.)) + { + const T x = -b / (T(2.) * a); + if (_interval.Contains(x)) + { + const T y = this->Evaluate(x); + if (y < yMin) + { + _xMin = x; + yMin = y; + } + } + } + } + return yMin; + } + + /// \brief Compute polynomial minimum in an `_interval` + /// \param[in] _interval polynomial argument interval to check + /// \return the polynomial minimum in the given interval (may + /// not be finite), or NaN if the interval is empty + public: T Minimum(const Interval &_interval) const + { + T xMin; + return this->Minimum(_interval, xMin); + } + + /// \brief Compute polynomial minimum + /// \param[out] _xMin polynomial argument that yields minimum + /// \return the polynomial minimum (may not be finite) + public: T Minimum(T &_xMin) const + { + return this->Minimum(Interval::Unbounded, _xMin); + } + + /// \brief Compute polynomial minimum + /// \return the polynomial minimum (may not be finite) + public: T Minimum() const + { + T xMin; + return this->Minimum(Interval::Unbounded, xMin); + } + + /// \brief Prints polynomial as p(`_x`) to `_out` stream + /// \param[in] _out Output stream to print to + /// \param[in] _x Argument name to be used + public: void Print(std::ostream &_out, const std::string &_x = "x") const + { + constexpr T epsilon = + std::numeric_limits::epsilon(); + bool streamStarted = false; + for (size_t i = 0; i < 4; ++i) + { + using std::abs; // enable ADL + const T magnitude = abs(this->coeffs[i]); + const bool sign = this->coeffs[i] < T(0); + const int exponent = 3 - i; + if (magnitude >= epsilon) + { + if (streamStarted) + { + if (sign) + { + _out << " - "; + } + else + { + _out << " + "; + } + } + else if (sign) + { + _out << "-"; + } + if (exponent > 0) + { + if ((magnitude - T(1)) > epsilon) + { + _out << magnitude << " "; + } + _out << _x; + if (exponent > 1) + { + _out << "^" << exponent; + } + } + else + { + _out << magnitude; + } + streamStarted = true; + } + } + if (!streamStarted) + { + _out << this->coeffs[3]; + } + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _p Polynomial3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Polynomial3 &_p) + { + _p.Print(_out, "x"); + return _out; + } + + /// \brief Polynomial coefficients + private: Vector4 coeffs; + }; + using Polynomial3f = Polynomial3; + using Polynomial3d = Polynomial3; + } + } +} + +#endif diff --git a/include/gz/math/Pose3.hh b/include/gz/math/Pose3.hh new file mode 100644 index 000000000..aabf9ec43 --- /dev/null +++ b/include/gz/math/Pose3.hh @@ -0,0 +1,500 @@ +/* + * Copyright (C) 2012 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_POSE_HH_ +#define GZ_MATH_POSE_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Pose3 Pose3.hh ignition/math/Pose3.hh + /// \brief Encapsulates a position and rotation in three space + template + class Pose3 + { + /// \brief math::Pose3(0, 0, 0, 0, 0, 0) + public: static const Pose3 Zero; + + /// \brief Default constructors + public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0) + { + } + + /// \brief Constructor + /// \param[in] _pos A position + /// \param[in] _rot A rotation + public: Pose3(const Vector3 &_pos, const Quaternion &_rot) + : p(_pos), q(_rot) + { + } + + /// \brief Constructor + /// \param[in] _x x position in meters. + /// \param[in] _y y position in meters. + /// \param[in] _z z position in meters. + /// \param[in] _roll Roll (rotation about X-axis) in radians. + /// \param[in] _pitch Pitch (rotation about y-axis) in radians. + /// \param[in] _yaw Yaw (rotation about z-axis) in radians. + public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) + : p(_x, _y, _z), q(_roll, _pitch, _yaw) + { + } + + /// \brief Constructor + /// \param[in] _x x position in meters. + /// \param[in] _y y position in meters. + /// \param[in] _z z position in meters. + /// \param[in] _qw Quaternion w value. + /// \param[in] _qx Quaternion x value. + /// \param[in] _qy Quaternion y value. + /// \param[in] _qz Quaternion z value. + public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz) + : p(_x, _y, _z), q(_qw, _qx, _qy, _qz) + { + } + + /// \brief Copy constructor + /// \param[in] _pose Pose3 to copy + public: Pose3(const Pose3 &_pose) + : p(_pose.p), q(_pose.q) + { + } + + /// \brief Destructor + public: virtual ~Pose3() + { + } + + /// \brief Set the pose from a Vector3 and a Quaternion + /// \param[in] _pos The position. + /// \param[in] _rot The rotation. + public: void Set(const Vector3 &_pos, const Quaternion &_rot) + { + this->p = _pos; + this->q = _rot; + } + + /// \brief Set the pose from pos and rpy vectors + /// \param[in] _pos The position. + /// \param[in] _rpy The rotation expressed as Euler angles. + public: void Set(const Vector3 &_pos, const Vector3 &_rpy) + { + this->p = _pos; + this->q.Euler(_rpy); + } + + /// \brief Set the pose from a six tuple. + /// \param[in] _x x position in meters. + /// \param[in] _y y position in meters. + /// \param[in] _z z position in meters. + /// \param[in] _roll Roll (rotation about X-axis) in radians. + /// \param[in] _pitch Pitch (rotation about y-axis) in radians. + /// \param[in] _yaw Pitch (rotation about z-axis) in radians. + public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) + { + this->p.Set(_x, _y, _z); + this->q.Euler(math::Vector3(_roll, _pitch, _yaw)); + } + + /// \brief See if a pose is finite (e.g., not nan) + public: bool IsFinite() const + { + return this->p.IsFinite() && this->q.IsFinite(); + } + + /// \brief Fix any nan values + public: inline void Correct() + { + this->p.Correct(); + this->q.Correct(); + } + + /// \brief Get the inverse of this pose + /// \return the inverse pose + public: Pose3 Inverse() const + { + Quaternion inv = this->q.Inverse(); + return Pose3(inv * (this->p*-1), inv); + } + + /// \brief Addition operator + /// A is the transform from O to P specified in frame O + /// B is the transform from P to Q specified in frame P + /// then, B + A is the transform from O to Q specified in frame O + /// \param[in] _pose Pose3 to add to this pose + /// \return The resulting pose + public: Pose3 operator+(const Pose3 &_pose) const + { + Pose3 result; + + result.p = this->CoordPositionAdd(_pose); + result.q = this->CoordRotationAdd(_pose.q); + + return result; + } + + /// \brief Add-Equals operator + /// \param[in] _pose Pose3 to add to this pose + /// \return The resulting pose + public: const Pose3 &operator+=(const Pose3 &_pose) + { + this->p = this->CoordPositionAdd(_pose); + this->q = this->CoordRotationAdd(_pose.q); + + return *this; + } + + /// \brief Negation operator + /// A is the transform from O to P in frame O + /// then -A is transform from P to O specified in frame P + /// \return The resulting pose + public: inline Pose3 operator-() const + { + return Pose3() - *this; + } + + /// \brief Subtraction operator + /// A is the transform from O to P in frame O + /// B is the transform from O to Q in frame O + /// B - A is the transform from P to Q in frame P + /// \param[in] _pose Pose3 to subtract from this one + /// \return The resulting pose + public: inline Pose3 operator-(const Pose3 &_pose) const + { + return Pose3(this->CoordPositionSub(_pose), + this->CoordRotationSub(_pose.q)); + } + + /// \brief Subtraction operator + /// \param[in] _pose Pose3 to subtract from this one + /// \return The resulting pose + public: const Pose3 &operator-=(const Pose3 &_pose) + { + this->p = this->CoordPositionSub(_pose); + this->q = this->CoordRotationSub(_pose.q); + + return *this; + } + + /// \brief Equality operator + /// \param[in] _pose Pose3 for comparison + /// \return True if equal + public: bool operator==(const Pose3 &_pose) const + { + return this->p == _pose.p && this->q == _pose.q; + } + + /// \brief Inequality operator + /// \param[in] _pose Pose3 for comparison + /// \return True if not equal + public: bool operator!=(const Pose3 &_pose) const + { + return this->p != _pose.p || this->q != _pose.q; + } + + /// \brief Multiplication operator. + /// Given X_OP (frame P relative to O) and X_PQ (frame Q relative to P) + /// then X_OQ = X_OP * X_PQ (frame Q relative to O). + /// \param[in] _pose The pose to multiply by. + /// \return The resulting pose. + public: Pose3 operator*(const Pose3 &_pose) const + { + return Pose3(_pose.CoordPositionAdd(*this), this->q * _pose.q); + } + + /// \brief Multiplication assignment operator. This pose will become + /// equal to this * _pose. + /// \param[in] _pose Pose3 to multiply to this pose + /// \return The resulting pose + public: const Pose3 &operator*=(const Pose3 &_pose) + { + *this = *this * _pose; + return *this; + } + + /// \brief Assignment operator + /// \param[in] _pose Pose3 to copy + public: Pose3 &operator=(const Pose3 &_pose) + { + this->p = _pose.p; + this->q = _pose.q; + return *this; + } + + /// \brief Add one point to a vector: result = this + pos + /// \param[in] _pos Position to add to this pose + /// \return the resulting position + public: Vector3 CoordPositionAdd(const Vector3 &_pos) const + { + Quaternion tmp(0.0, _pos.X(), _pos.Y(), _pos.Z()); + + // result = pose.q + pose.q * this->p * pose.q! + tmp = this->q * (tmp * this->q.Inverse()); + + return Vector3(this->p.X() + tmp.X(), + this->p.Y() + tmp.Y(), + this->p.Z() + tmp.Z()); + } + + /// \brief Add one point to another: result = this + pose + /// \param[in] _pose The Pose3 to add + /// \return The resulting position + public: Vector3 CoordPositionAdd(const Pose3 &_pose) const + { + Quaternion tmp(static_cast(0), + this->p.X(), this->p.Y(), this->p.Z()); + + // result = _pose.q + _pose.q * this->p * _pose.q! + tmp = _pose.q * (tmp * _pose.q.Inverse()); + + return Vector3(_pose.p.X() + tmp.X(), + _pose.p.Y() + tmp.Y(), + _pose.p.Z() + tmp.Z()); + } + + /// \brief Subtract one position from another: result = this - pose + /// \param[in] _pose Pose3 to subtract + /// \return The resulting position + public: inline Vector3 CoordPositionSub(const Pose3 &_pose) const + { + Quaternion tmp(0, + this->p.X() - _pose.p.X(), + this->p.Y() - _pose.p.Y(), + this->p.Z() - _pose.p.Z()); + + tmp = _pose.q.Inverse() * (tmp * _pose.q); + return Vector3(tmp.X(), tmp.Y(), tmp.Z()); + } + + /// \brief Add one rotation to another: result = this->q + rot + /// \param[in] _rot Rotation to add + /// \return The resulting rotation + public: Quaternion CoordRotationAdd(const Quaternion &_rot) const + { + return Quaternion(_rot * this->q); + } + + /// \brief Subtract one rotation from another: result = this->q - rot + /// \param[in] _rot The rotation to subtract + /// \return The resulting rotation + public: inline Quaternion CoordRotationSub( + const Quaternion &_rot) const + { + Quaternion result(_rot.Inverse() * this->q); + result.Normalize(); + return result; + } + + /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and + /// this, find a + /// \param[in] _b the other pose + public: Pose3 CoordPoseSolve(const Pose3 &_b) const + { + Quaternion qt; + Pose3 a; + + a.q = this->q.Inverse() * _b.q; + qt = a.q * Quaternion(0, this->p.X(), this->p.Y(), this->p.Z()); + qt = qt * a.q.Inverse(); + a.p = _b.p - Vector3(qt.X(), qt.Y(), qt.Z()); + + return a; + } + + /// \brief Reset the pose + public: void Reset() + { + // set the position to zero + this->p.Set(); + this->q = Quaternion::Identity; + } + + /// \brief Rotate vector part of a pose about the origin + /// \param[in] _q rotation + /// \return the rotated pose + public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const + { + Pose3 a = *this; + a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X() + +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y() + +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z()); + a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X() + +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y() + +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z()); + a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X() + +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y() + +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z()); + return a; + } + + /// \brief Round all values to _precision decimal places + /// \param[in] _precision + public: void Round(int _precision) + { + this->q.Round(_precision); + this->p.Round(_precision); + } + + /// \brief Get the position. + /// \return Origin of the pose. + public: inline const Vector3 &Pos() const + { + return this->p; + } + + /// \brief Get a mutable reference to the position. + /// \return Origin of the pose. + public: inline Vector3 &Pos() + { + return this->p; + } + + /// \brief Get the X value of the position. + /// \return Value X of the origin of the pose. + /// \note The return is made by value since + /// Vector3.X() is already a reference. + public: inline const T X() const + { + return this->p.X(); + } + + /// \brief Set X value of the position. + public: inline void SetX(T x) + { + this->p.X() = x; + } + + /// \brief Get the Y value of the position. + /// \return Value Y of the origin of the pose. + /// \note The return is made by value since + /// Vector3.Y() is already a reference. + public: inline const T Y() const + { + return this->p.Y(); + } + + /// \brief Set the Y value of the position. + public: inline void SetY(T y) + { + this->p.Y() = y; + } + + /// \brief Get the Z value of the position. + /// \return Value Z of the origin of the pose. + /// \note The return is made by value since + /// Vector3.Z() is already a reference. + public: inline const T Z() const + { + return this->p.Z(); + } + + /// \brief Set the Z value of the position. + public: inline void SetZ(T z) + { + this->p.Z() = z; + } + + /// \brief Get the rotation. + /// \return Quaternion representation of the rotation. + public: inline const Quaternion &Rot() const + { + return this->q; + } + + /// \brief Get a mutable reference to the rotation. + /// \return Quaternion representation of the rotation. + public: inline Quaternion &Rot() + { + return this->q; + } + + /// \brief Get the Roll value of the rotation. + /// \return Roll value of the orientation. + /// \note The return is made by value since + /// Quaternion.Roll() is already a reference. + public: inline const T Roll() const + { + return this->q.Roll(); + } + + /// \brief Get the Pitch value of the rotation. + /// \return Pitch value of the orientation. + /// \note The return is made by value since + /// Quaternion.Pitch() is already a reference. + public: inline const T Pitch() const + { + return this->q.Pitch(); + } + + /// \brief Get the Yaw value of the rotation. + /// \return Yaw value of the orientation. + /// \note The return is made by value since + /// Quaternion.Yaw() is already a reference. + public: inline const T Yaw() const + { + return this->q.Yaw(); + } + + /// \brief Stream insertion operator + /// \param[in] _out output stream + /// \param[in] _pose pose to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Pose3 &_pose) + { + _out << _pose.Pos() << " " << _pose.Rot(); + return _out; + } + + /// \brief Stream extraction operator + /// \param[in] _in the input stream + /// \param[in] _pose the pose + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, gz::math::Pose3 &_pose) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + Vector3 pos; + Quaternion rot; + _in >> pos >> rot; + _pose.Set(pos, rot); + return _in; + } + + /// \brief The position + private: Vector3 p; + + /// \brief The rotation + private: Quaternion q; + }; + template const Pose3 Pose3::Zero(0, 0, 0, 0, 0, 0); + + typedef Pose3 Pose3i; + typedef Pose3 Pose3d; + typedef Pose3 Pose3f; + } + } +} +#endif diff --git a/include/gz/math/Quaternion.hh b/include/gz/math/Quaternion.hh new file mode 100644 index 000000000..b55f14f1c --- /dev/null +++ b/include/gz/math/Quaternion.hh @@ -0,0 +1,1107 @@ +/* + * Copyright (C) 2012 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_QUATERNION_HH_ +#define GZ_MATH_QUATERNION_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + template class Matrix3; + + /// \class Quaternion Quaternion.hh ignition/math/Quaternion.hh + /// \brief A quaternion class + template + class Quaternion + { + /// \brief math::Quaternion(1, 0, 0, 0) + public: static const Quaternion Identity; + + /// \brief math::Quaternion(0, 0, 0, 0) + public: static const Quaternion Zero; + + /// \brief Default Constructor + public: Quaternion() + : qw(1), qx(0), qy(0), qz(0) + { + // quaternion not normalized, because that breaks + // Pose::CoordPositionAdd(...) + } + + /// \brief Constructor + /// \param[in] _w W param + /// \param[in] _x X param + /// \param[in] _y Y param + /// \param[in] _z Z param + public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z) + : qw(_w), qx(_x), qy(_y), qz(_z) + {} + + /// \brief Constructor from Euler angles in radians + /// \param[in] _roll roll + /// \param[in] _pitch pitch + /// \param[in] _yaw yaw + public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw) + { + this->Euler(Vector3(_roll, _pitch, _yaw)); + } + + /// \brief Constructor from axis angle + /// \param[in] _axis the rotation axis + /// \param[in] _angle the rotation angle in radians + public: Quaternion(const Vector3 &_axis, const T &_angle) + { + this->Axis(_axis, _angle); + } + + /// \brief Constructor + /// \param[in] _rpy euler angles, in radians + public: explicit Quaternion(const Vector3 &_rpy) + { + this->Euler(_rpy); + } + + /// \brief Construct from rotation matrix. + /// \param[in] _mat rotation matrix (must be orthogonal, the function + /// doesn't check it) + public: explicit Quaternion(const Matrix3 &_mat) + { + this->Matrix(_mat); + } + + /// \brief Copy constructor + /// \param[in] _qt Quaternion to copy + public: Quaternion(const Quaternion &_qt) + { + this->qw = _qt.qw; + this->qx = _qt.qx; + this->qy = _qt.qy; + this->qz = _qt.qz; + } + + /// \brief Destructor + public: ~Quaternion() {} + + /// \brief Assignment operator + /// \param[in] _qt Quaternion to copy + public: Quaternion &operator=(const Quaternion &_qt) + { + this->qw = _qt.qw; + this->qx = _qt.qx; + this->qy = _qt.qy; + this->qz = _qt.qz; + + return *this; + } + + /// \brief Invert the quaternion + public: void Invert() + { + this->Normalize(); + // this->qw = this->qw; + this->qx = -this->qx; + this->qy = -this->qy; + this->qz = -this->qz; + } + + /// \brief Get the inverse of this quaternion + /// \return Inverse quaternion + public: inline Quaternion Inverse() const + { + T s = 0; + Quaternion q(this->qw, this->qx, this->qy, this->qz); + + // use s to test if quaternion is valid + s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz; + + if (equal(s, static_cast(0))) + { + q.qw = 1.0; + q.qx = 0.0; + q.qy = 0.0; + q.qz = 0.0; + } + else + { + // deal with non-normalized quaternion + // div by s so q * qinv = identity + q.qw = q.qw / s; + q.qx = -q.qx / s; + q.qy = -q.qy / s; + q.qz = -q.qz / s; + } + return q; + } + + /// \brief Return the logarithm + /// \return the log + public: Quaternion Log() const + { + // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length, + // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) = + // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1. + + Quaternion result; + result.qw = 0.0; + + if (std::abs(this->qw) < 1.0) + { + T fAngle = acos(this->qw); + T fSin = sin(fAngle); + if (std::abs(fSin) >= 1e-3) + { + T fCoeff = fAngle/fSin; + result.qx = fCoeff*this->qx; + result.qy = fCoeff*this->qy; + result.qz = fCoeff*this->qz; + return result; + } + } + + result.qx = this->qx; + result.qy = this->qy; + result.qz = this->qz; + + return result; + } + + /// \brief Return the exponent + /// \return the exp + public: Quaternion Exp() const + { + // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then + // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero, + // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1. + + T fAngle = sqrt(this->qx*this->qx+ + this->qy*this->qy+this->qz*this->qz); + T fSin = sin(fAngle); + + Quaternion result; + result.qw = cos(fAngle); + + if (std::abs(fSin) >= 1e-3) + { + T fCoeff = fSin/fAngle; + result.qx = fCoeff*this->qx; + result.qy = fCoeff*this->qy; + result.qz = fCoeff*this->qz; + } + else + { + result.qx = this->qx; + result.qy = this->qy; + result.qz = this->qz; + } + + return result; + } + + /// \brief Normalize the quaternion + public: void Normalize() + { + T s = 0; + + s = T(sqrt(this->qw * this->qw + this->qx * this->qx + + this->qy * this->qy + this->qz * this->qz)); + + if (equal(s, static_cast(0))) + { + this->qw = T(1.0); + this->qx = T(0.0); + this->qy = T(0.0); + this->qz = T(0.0); + } + else + { + this->qw /= s; + this->qx /= s; + this->qy /= s; + this->qz /= s; + } + } + + /// \brief Gets a normalized version of this quaternion + /// \return a normalized quaternion + public: Quaternion Normalized() const + { + Quaternion result = *this; + result.Normalize(); + return result; + } + + /// \brief Set the quaternion from an axis and angle + /// \param[in] _ax X axis + /// \param[in] _ay Y axis + /// \param[in] _az Z axis + /// \param[in] _aa Angle in radians + public: void Axis(T _ax, T _ay, T _az, T _aa) + { + T l; + + l = _ax * _ax + _ay * _ay + _az * _az; + + if (equal(l, static_cast(0))) + { + this->qw = 1; + this->qx = 0; + this->qy = 0; + this->qz = 0; + } + else + { + _aa *= 0.5; + l = sin(_aa) / sqrt(l); + this->qw = cos(_aa); + this->qx = _ax * l; + this->qy = _ay * l; + this->qz = _az * l; + } + + this->Normalize(); + } + + /// \brief Set the quaternion from an axis and angle + /// \param[in] _axis Axis + /// \param[in] _a Angle in radians + public: void Axis(const Vector3 &_axis, T _a) + { + this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a); + } + + /// \brief Set this quaternion from 4 floating numbers + /// \param[in] _w w + /// \param[in] _x x + /// \param[in] _y y + /// \param[in] _z z + public: void Set(T _w, T _x, T _y, T _z) + { + this->qw = _w; + this->qx = _x; + this->qy = _y; + this->qz = _z; + } + + /// \brief Set the quaternion from Euler angles. The order of operations + /// is roll, pitch, yaw around a fixed body frame axis + /// (the original frame of the object before rotation is applied). + /// Roll is a rotation about x, pitch is about y, yaw is about z. + /// \param[in] _vec Euler angle + public: void Euler(const Vector3 &_vec) + { + this->Euler(_vec.X(), _vec.Y(), _vec.Z()); + } + + /// \brief Set the quaternion from Euler angles. + /// \param[in] _roll Roll angle (radians). + /// \param[in] _pitch Pitch angle (radians). + /// \param[in] _yaw Yaw angle (radians). + public: void Euler(T _roll, T _pitch, T _yaw) + { + T phi, the, psi; + + phi = _roll / T(2.0); + the = _pitch / T(2.0); + psi = _yaw / T(2.0); + + this->qw = T(cos(phi) * cos(the) * cos(psi) + + sin(phi) * sin(the) * sin(psi)); + this->qx = T(sin(phi) * cos(the) * cos(psi) - + cos(phi) * sin(the) * sin(psi)); + this->qy = T(cos(phi) * sin(the) * cos(psi) + + sin(phi) * cos(the) * sin(psi)); + this->qz = T(cos(phi) * cos(the) * sin(psi) - + sin(phi) * sin(the) * cos(psi)); + + this->Normalize(); + } + + /// \brief Return the rotation in Euler angles + /// \return This quaternion as an Euler vector + public: Vector3 Euler() const + { + Vector3 vec; + + T tol = static_cast(1e-15); + + Quaternion copy = *this; + T squ; + T sqx; + T sqy; + T sqz; + + copy.Normalize(); + + squ = copy.qw * copy.qw; + sqx = copy.qx * copy.qx; + sqy = copy.qy * copy.qy; + sqz = copy.qz * copy.qz; + + // Pitch + T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy); + if (sarg <= T(-1.0)) + { + vec.Y(T(-0.5*IGN_PI)); + } + else if (sarg >= T(1.0)) + { + vec.Y(T(0.5*IGN_PI)); + } + else + { + vec.Y(T(asin(sarg))); + } + + // If the pitch angle is PI/2 or -PI/2, we can only compute + // the sum roll + yaw. However, any combination that gives + // the right sum will produce the correct orientation, so we + // set yaw = 0 and compute roll. + // pitch angle is PI/2 + if (std::abs(sarg - 1) < tol) + { + vec.Z(0); + vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw), + squ - sqx + sqy - sqz))); + } + // pitch angle is -PI/2 + else if (std::abs(sarg + 1) < tol) + { + vec.Z(0); + vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw), + squ - sqx + sqy - sqz))); + } + else + { + // Roll + vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx), + squ - sqx - sqy + sqz))); + + // Yaw + vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz), + squ + sqx - sqy - sqz))); + } + + return vec; + } + + /// \brief Convert euler angles to quatern. + /// \param[in] _vec The vector of angles to convert. + /// \return The converted quaternion. + public: static Quaternion EulerToQuaternion(const Vector3 &_vec) + { + Quaternion result; + result.Euler(_vec); + return result; + } + + /// \brief Convert euler angles to quatern. + /// \param[in] _x rotation along x + /// \param[in] _y rotation along y + /// \param[in] _z rotation along z + /// \return The converted quaternion. + public: static Quaternion EulerToQuaternion(T _x, T _y, T _z) + { + return EulerToQuaternion(Vector3(_x, _y, _z)); + } + + /// \brief Get the Euler roll angle in radians + /// \return the roll component + public: T Roll() const + { + return this->Euler().X(); + } + + /// \brief Get the Euler pitch angle in radians + /// \return the pitch component + public: T Pitch() const + { + return this->Euler().Y(); + } + + /// \brief Get the Euler yaw angle in radians + /// \return the yaw component + public: T Yaw() const + { + return this->Euler().Z(); + } + + /// \brief Return rotation as axis and angle + /// \param[out] _axis rotation axis + /// \param[out] _angle ccw angle in radians + public: void ToAxis(Vector3 &_axis, T &_angle) const + { + T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz; + if (equal(len, static_cast(0))) + { + _angle = 0.0; + _axis.Set(1, 0, 0); + } + else + { + _angle = 2.0 * acos(this->qw); + T invLen = 1.0 / sqrt(len); + _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen); + } + } + + /// \brief Set from a rotation matrix. + /// \param[in] _mat rotation matrix (must be orthogonal, the function + /// doesn't check it) + /// + /// Implementation inspired by + /// http://www.euclideanspace.com/maths/geometry/rotations/ + /// conversions/matrixToQuaternion/ + void Matrix(const Matrix3 &_mat) + { + const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2); + if (trace > 0.0000001) + { + qw = sqrt(1 + trace) / 2; + const T s = 1.0 / (4 * qw); + qx = (_mat(2, 1) - _mat(1, 2)) * s; + qy = (_mat(0, 2) - _mat(2, 0)) * s; + qz = (_mat(1, 0) - _mat(0, 1)) * s; + } + else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2)) + { + qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2; + const T s = 1.0 / (4 * qx); + qw = (_mat(2, 1) - _mat(1, 2)) * s; + qy = (_mat(1, 0) + _mat(0, 1)) * s; + qz = (_mat(0, 2) + _mat(2, 0)) * s; + } + else if (_mat(1, 1) > _mat(2, 2)) + { + qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2; + const T s = 1.0 / (4 * qy); + qw = (_mat(0, 2) - _mat(2, 0)) * s; + qx = (_mat(0, 1) + _mat(1, 0)) * s; + qz = (_mat(1, 2) + _mat(2, 1)) * s; + } + else + { + qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2; + const T s = 1.0 / (4 * qz); + qw = (_mat(1, 0) - _mat(0, 1)) * s; + qx = (_mat(0, 2) + _mat(2, 0)) * s; + qy = (_mat(1, 2) + _mat(2, 1)) * s; + } + } + + /// \brief Set this quaternion 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 + /// + /// Implementation inspired by + /// http://stackoverflow.com/a/11741520/1076564 + public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2) + { + // generally, we utilize the fact that a quat (w, x, y, z) represents + // rotation of angle 2*w about axis (x, y, z) + // + // so we want to take get a vector half-way between no rotation and the + // double rotation, which is + // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2 + // if _v1 and _v2 are unit quaternions + // + // since we normalize the result anyway, we can omit the division, + // getting the result: + // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized() + // + // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2) + // is multiplied by k = norm(_v1)*norm(_v2) + + const T kCosTheta = _v1.Dot(_v2); + const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength()); + + if (fabs(kCosTheta/k + 1) < 1e-6) + { + // the vectors are opposite + // any vector orthogonal to _v1 + Vector3 other; + { + const Vector3 _v1Abs(_v1.Abs()); + if (_v1Abs.X() < _v1Abs.Y()) + { + if (_v1Abs.X() < _v1Abs.Z()) + { + other.Set(1, 0, 0); + } + else + { + other.Set(0, 0, 1); + } + } + else + { + if (_v1Abs.Y() < _v1Abs.Z()) + { + other.Set(0, 1, 0); + } + else + { + other.Set(0, 0, 1); + } + } + } + + const Vector3 axis(_v1.Cross(other).Normalize()); + + qw = 0; + qx = axis.X(); + qy = axis.Y(); + qz = axis.Z(); + } + else + { + // the vectors are in general position + const Vector3 axis(_v1.Cross(_v2)); + qw = kCosTheta + k; + qx = axis.X(); + qy = axis.Y(); + qz = axis.Z(); + this->Normalize(); + } + } + + /// \brief Scale a Quaternion + /// \param[in] _scale Amount to scale this rotation + public: void Scale(T _scale) + { + Quaternion b; + Vector3 axis; + T angle; + + // Convert to axis-and-angle + this->ToAxis(axis, angle); + angle *= _scale; + + this->Axis(axis.X(), axis.Y(), axis.Z(), angle); + } + + /// \brief Addition operator + /// \param[in] _qt quaternion for addition + /// \return this quaternion + _qt + public: Quaternion operator+(const Quaternion &_qt) const + { + Quaternion result(this->qw + _qt.qw, this->qx + _qt.qx, + this->qy + _qt.qy, this->qz + _qt.qz); + return result; + } + + /// \brief Addition operator + /// \param[in] _qt quaternion for addition + /// \return this quaternion + qt + public: Quaternion operator+=(const Quaternion &_qt) + { + *this = *this + _qt; + + return *this; + } + + /// \brief Subtraction operator + /// \param[in] _qt quaternion to subtract + /// \return this quaternion - _qt + public: Quaternion operator-(const Quaternion &_qt) const + { + Quaternion result(this->qw - _qt.qw, this->qx - _qt.qx, + this->qy - _qt.qy, this->qz - _qt.qz); + return result; + } + + /// \brief Subtraction operator + /// \param[in] _qt Quaternion for subtraction + /// \return This quaternion - qt + public: Quaternion operator-=(const Quaternion &_qt) + { + *this = *this - _qt; + return *this; + } + + /// \brief Multiplication operator + /// \param[in] _q Quaternion for multiplication + /// \return This quaternion multiplied by the parameter + public: inline Quaternion operator*(const Quaternion &_q) const + { + return Quaternion( + this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz, + this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy, + this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx, + this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw); + } + + /// \brief Multiplication operator by a scalar. + /// \param[in] _f factor + /// \return quaternion multiplied by the scalar + public: Quaternion operator*(const T &_f) const + { + return Quaternion(this->qw*_f, this->qx*_f, + this->qy*_f, this->qz*_f); + } + + /// \brief Multiplication operator + /// \param[in] _qt Quaternion for multiplication + /// \return This quaternion multiplied by the parameter + public: Quaternion operator*=(const Quaternion &_qt) + { + *this = *this * _qt; + return *this; + } + + /// \brief Vector3 multiplication operator + /// \param[in] _v vector to multiply + /// \return The result of the vector multiplication + public: Vector3 operator*(const Vector3 &_v) const + { + Vector3 uv, uuv; + Vector3 qvec(this->qx, this->qy, this->qz); + uv = qvec.Cross(_v); + uuv = qvec.Cross(uv); + uv *= (2.0f * this->qw); + uuv *= 2.0f; + + return _v + uv + uuv; + } + + /// \brief Equality test with tolerance. + /// \param[in] _qt Quaternion for comparison + /// \param[in] _tol equality tolerance + /// \return true if the elements of the quaternions are equal + /// within the tolerance specified by _tol, false otherwise + public: bool Equal(const Quaternion &_qt, const T &_tol) const + { + return equal(this->qx, _qt.qx, _tol) && + equal(this->qy, _qt.qy, _tol) && + equal(this->qz, _qt.qz, _tol) && + equal(this->qw, _qt.qw, _tol); + } + + /// \brief Equal to operator + /// \param[in] _qt Quaternion for comparison + /// \return True if equal + public: bool operator==(const Quaternion &_qt) const + { + return this->Equal(_qt, static_cast(0.001)); + } + + /// \brief Not equal to operator + /// \param[in] _qt Quaternion for comparison + /// \return True if not equal + public: bool operator!=(const Quaternion &_qt) const + { + return !(*this == _qt); + } + + /// \brief Unary minus operator + /// \return negates each component of the quaternion + public: Quaternion operator-() const + { + return Quaternion(-this->qw, -this->qx, -this->qy, -this->qz); + } + + /// \brief Rotate a vector using the quaternion + /// \param[in] _vec vector to rotate + /// \return the rotated vector + public: inline Vector3 RotateVector(const Vector3 &_vec) const + { + Quaternion tmp(static_cast(0), + _vec.X(), _vec.Y(), _vec.Z()); + tmp = (*this) * (tmp * this->Inverse()); + return Vector3(tmp.qx, tmp.qy, tmp.qz); + } + + /// \brief Do the reverse rotation of a vector by this quaternion + /// \param[in] _vec the vector + /// \return the reversed vector + public: Vector3 RotateVectorReverse(const Vector3 &_vec) const + { + Quaternion tmp(0.0, _vec.X(), _vec.Y(), _vec.Z()); + + tmp = this->Inverse() * (tmp * (*this)); + + return Vector3(tmp.qx, tmp.qy, tmp.qz); + } + + /// \brief See if a quaternion is finite (e.g., not nan) + /// \return True if quaternion is finite + public: bool IsFinite() const + { + // std::isfinite works with floating point values, need to explicit + // cast to avoid ambiguity in vc++. + return std::isfinite(static_cast(this->qw)) && + std::isfinite(static_cast(this->qx)) && + std::isfinite(static_cast(this->qy)) && + std::isfinite(static_cast(this->qz)); + } + + /// \brief Correct any nan values in this quaternion + public: inline void Correct() + { + // std::isfinite works with floating point values, need to explicit + // cast to avoid ambiguity in vc++. + if (!std::isfinite(static_cast(this->qx))) + this->qx = 0; + if (!std::isfinite(static_cast(this->qy))) + this->qy = 0; + if (!std::isfinite(static_cast(this->qz))) + this->qz = 0; + if (!std::isfinite(static_cast(this->qw))) + this->qw = 1; + + if (equal(this->qw, static_cast(0)) && + equal(this->qx, static_cast(0)) && + equal(this->qy, static_cast(0)) && + equal(this->qz, static_cast(0))) + { + this->qw = 1; + } + } + + /// \brief Return the X axis + /// \return the X axis of the vector + public: Vector3 XAxis() const + { + T fTy = 2.0f*this->qy; + T fTz = 2.0f*this->qz; + + T fTwy = fTy*this->qw; + T fTwz = fTz*this->qw; + T fTxy = fTy*this->qx; + T fTxz = fTz*this->qx; + T fTyy = fTy*this->qy; + T fTzz = fTz*this->qz; + + return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy); + } + + /// \brief Return the Y axis + /// \return the Y axis of the vector + public: Vector3 YAxis() const + { + T fTx = 2.0f*this->qx; + T fTy = 2.0f*this->qy; + T fTz = 2.0f*this->qz; + T fTwx = fTx*this->qw; + T fTwz = fTz*this->qw; + T fTxx = fTx*this->qx; + T fTxy = fTy*this->qx; + T fTyz = fTz*this->qy; + T fTzz = fTz*this->qz; + + return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx); + } + + /// \brief Return the Z axis + /// \return the Z axis of the vector + public: Vector3 ZAxis() const + { + T fTx = 2.0f*this->qx; + T fTy = 2.0f*this->qy; + T fTz = 2.0f*this->qz; + T fTwx = fTx*this->qw; + T fTwy = fTy*this->qw; + T fTxx = fTx*this->qx; + T fTxz = fTz*this->qx; + T fTyy = fTy*this->qy; + T fTyz = fTz*this->qy; + + return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy)); + } + + /// \brief Round all values to _precision decimal places + /// \param[in] _precision the precision + public: void Round(int _precision) + { + this->qx = precision(this->qx, _precision); + this->qy = precision(this->qy, _precision); + this->qz = precision(this->qz, _precision); + this->qw = precision(this->qw, _precision); + } + + /// \brief Dot product + /// \param[in] _q the other quaternion + /// \return the product + public: T Dot(const Quaternion &_q) const + { + return this->qw*_q.qw + this->qx * _q.qx + + this->qy*_q.qy + this->qz*_q.qz; + } + + /// \brief Spherical quadratic interpolation + /// given the ends and an interpolation parameter between 0 and 1 + /// \param[in] _fT the interpolation parameter + /// \param[in] _rkP the beginning quaternion + /// \param[in] _rkA first intermediate quaternion + /// \param[in] _rkB second intermediate quaternion + /// \param[in] _rkQ the end quaternion + /// \param[in] _shortestPath when true, the rotation may be inverted to + /// get to minimize rotation + /// \return The result of the quadratic interpolation + public: static Quaternion Squad(T _fT, + const Quaternion &_rkP, const Quaternion &_rkA, + const Quaternion &_rkB, const Quaternion &_rkQ, + bool _shortestPath = false) + { + T fSlerpT = 2.0f*_fT*(1.0f-_fT); + Quaternion kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath); + Quaternion kSlerpQ = Slerp(_fT, _rkA, _rkB); + return Slerp(fSlerpT, kSlerpP, kSlerpQ); + } + + /// \brief Spherical linear interpolation between 2 quaternions, + /// given the ends and an interpolation parameter between 0 and 1 + /// \param[in] _fT the interpolation parameter + /// \param[in] _rkP the beginning quaternion + /// \param[in] _rkQ the end quaternion + /// \param[in] _shortestPath when true, the rotation may be inverted to + /// get to minimize rotation + /// \return The result of the linear interpolation + public: static Quaternion Slerp(T _fT, + const Quaternion &_rkP, const Quaternion &_rkQ, + bool _shortestPath = false) + { + T fCos = _rkP.Dot(_rkQ); + Quaternion rkT; + + // Do we need to invert rotation? + if (fCos < 0.0f && _shortestPath) + { + fCos = -fCos; + rkT = -_rkQ; + } + else + { + rkT = _rkQ; + } + + if (std::abs(fCos) < 1 - 1e-03) + { + // Standard case (slerp) + T fSin = sqrt(1 - (fCos*fCos)); + T fAngle = atan2(fSin, fCos); + // FIXME: should check if (std::abs(fSin) >= 1e-3) + T fInvSin = 1.0f / fSin; + T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin; + T fCoeff1 = sin(_fT * fAngle) * fInvSin; + return _rkP * fCoeff0 + rkT * fCoeff1; + } + else + { + // There are two situations: + // 1. "rkP" and "rkQ" are very close (fCos ~= +1), + // so we can do a linear interpolation safely. + // 2. "rkP" and "rkQ" are almost inverse of each + // other (fCos ~= -1), there + // are an infinite number of possibilities interpolation. + // but we haven't have method to fix this case, so just use + // linear interpolation here. + Quaternion t = _rkP * (1.0f - _fT) + rkT * _fT; + // taking the complement requires renormalisation + t.Normalize(); + return t; + } + } + + /// \brief Integrate quaternion for constant angular velocity vector + /// along specified interval `_deltaT`. + /// Implementation based on: + /// http://physicsforgames.blogspot.com/2010/02/quaternions.html + /// \param[in] _angularVelocity Angular velocity vector, specified in + /// same reference frame as base of this quaternion. + /// \param[in] _deltaT Time interval in seconds to integrate over. + /// \return Quaternion at integrated configuration. + public: Quaternion Integrate(const Vector3 &_angularVelocity, + const T _deltaT) const + { + Quaternion deltaQ; + Vector3 theta = _angularVelocity * _deltaT / 2; + T thetaMagSq = theta.SquaredLength(); + T s; + if (thetaMagSq * thetaMagSq / 24.0 < MIN_D) + { + deltaQ.W() = 1.0 - thetaMagSq / 2.0; + s = 1.0 - thetaMagSq / 6.0; + } + else + { + double thetaMag = sqrt(thetaMagSq); + deltaQ.W() = cos(thetaMag); + s = sin(thetaMag) / thetaMag; + } + deltaQ.X() = theta.X() * s; + deltaQ.Y() = theta.Y() * s; + deltaQ.Z() = theta.Z() * s; + return deltaQ * (*this); + } + + /// \brief Get the w component. + /// \return The w quaternion component. + public: inline const T &W() const + { + return this->qw; + } + + /// \brief Get the x component. + /// \return The x quaternion component. + public: inline const T &X() const + { + return this->qx; + } + + /// \brief Get the y component. + /// \return The y quaternion component. + public: inline const T &Y() const + { + return this->qy; + } + + /// \brief Get the z component. + /// \return The z quaternion component. + public: inline const T &Z() const + { + return this->qz; + } + + + /// \brief Get a mutable w component. + /// \return The w quaternion component. + public: inline T &W() + { + return this->qw; + } + + /// \brief Get a mutable x component. + /// \return The x quaternion component. + public: inline T &X() + { + return this->qx; + } + + /// \brief Get a mutable y component. + /// \return The y quaternion component. + public: inline T &Y() + { + return this->qy; + } + + /// \brief Get a mutable z component. + /// \return The z quaternion component. + public: inline T &Z() + { + return this->qz; + } + + /// \brief Set the x component. + /// \param[in] _v The new value for the x quaternion component. + public: inline void X(T _v) + { + this->qx = _v; + } + + /// \brief Set the y component. + /// \param[in] _v The new value for the y quaternion component. + public: inline void Y(T _v) + { + this->qy = _v; + } + + /// \brief Set the z component. + /// \param[in] _v The new value for the z quaternion component. + public: inline void Z(T _v) + { + this->qz = _v; + } + + /// \brief Set the w component. + /// \param[in] _v The new value for the w quaternion component. + public: inline void W(T _v) + { + this->qw = _v; + } + + /// \brief Stream insertion operator + /// \param[in] _out output stream + /// \param[in] _q quaternion to output + /// \return the stream + public: friend std::ostream &operator<<(std::ostream &_out, + const gz::math::Quaternion &_q) + { + Vector3 v(_q.Euler()); + _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " " + << precision(v.Z(), 6); + return _out; + } + + /// \brief Stream extraction operator + /// \param[in] _in input stream + /// \param[in] _q Quaternion to read values into + /// \return The istream + public: friend std::istream &operator>>(std::istream &_in, + gz::math::Quaternion &_q) + { + Angle roll, pitch, yaw; + + // Skip white spaces + _in.setf(std::ios_base::skipws); + _in >> roll >> pitch >> yaw; + + if (!_in.fail()) + { + _q.Euler(Vector3(*roll, *pitch, *yaw)); + } + + return _in; + } + + /// \brief w value of the quaternion + private: T qw; + + /// \brief x value of the quaternion + private: T qx; + + /// \brief y value of the quaternion + private: T qy; + + /// \brief z value of the quaternion + private: T qz; + }; + + template const Quaternion + Quaternion::Identity(1, 0, 0, 0); + + template const Quaternion + Quaternion::Zero(0, 0, 0, 0); + + typedef Quaternion Quaterniond; + typedef Quaternion Quaternionf; + typedef Quaternion Quaternioni; + } + } +} +#endif diff --git a/include/gz/math/Rand.hh b/include/gz/math/Rand.hh new file mode 100644 index 000000000..a911f2be3 --- /dev/null +++ b/include/gz/math/Rand.hh @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2012 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_RAND_HH_ +#define GZ_MATH_RAND_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \def GeneratorType + /// \brief std::mt19937 + typedef std::mt19937 GeneratorType; + /// \def UniformRealDist + /// \brief std::uniform_real_distribution + typedef std::uniform_real_distribution UniformRealDist; + /// \def NormalRealDist + /// \brief std::normal_distribution + typedef std::normal_distribution NormalRealDist; + /// \def UniformIntDist + /// \brief std::uniform_int + typedef std::uniform_int_distribution UniformIntDist; + + /// \class Rand Rand.hh ignition/math/Rand.hh + /// \brief Random number generator class + class IGNITION_MATH_VISIBLE Rand + { + /// \brief Set the seed value. + /// \param[in] _seed The seed used to initialize the randon number + /// generator. + public: static void Seed(unsigned int _seed); + + /// \brief Get the seed value. + /// \return The seed value used to initialize the random number + /// generator. + public: static unsigned int Seed(); + + /// \brief Get a double from a uniform distribution + /// \param[in] _min Minimum bound for the random number + /// \param[in] _max Maximum bound for the random number + public: static double DblUniform(double _min = 0, double _max = 1); + + /// \brief Get a double from a normal distribution + /// \param[in] _mean Mean value for the distribution + /// \param[in] _sigma Sigma value for the distribution + public: static double DblNormal(double _mean = 0, double _sigma = 1); + + /// \brief Get an integer from a uniform distribution + /// \param[in] _min Minimum bound for the random number + /// \param[in] _max Maximum bound for the random number + public: static int32_t IntUniform(int _min, int _max); + + /// \brief Get an integer from a normal distribution + /// \param[in] _mean Mean value for the distribution + /// \param[in] _sigma Sigma value for the distribution + public: static int32_t IntNormal(int _mean, int _sigma); + + /// \brief Get a mutable reference to the seed (create the static + /// member if it hasn't been created yet). + private: static uint32_t &SeedMutable(); + + /// \brief Get a mutable reference to the random generator (create the + /// static member if it hasn't been created yet). + private: static GeneratorType &RandGenerator(); + }; + } + } +} +#endif diff --git a/include/gz/math/Region3.hh b/include/gz/math/Region3.hh new file mode 100644 index 000000000..70e2e43f9 --- /dev/null +++ b/include/gz/math/Region3.hh @@ -0,0 +1,209 @@ +/* + * 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_REGION3_HH_ +#define GZ_MATH_REGION3_HH_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Region3 Region3.hh ignition/math/Region3.hh + /// \brief The Region3 class represents the cartesian product + /// of intervals Ix ✕ Iy ✕ Iz, one per axis, yielding an + /// axis-aligned region of R^3 space. It can be thought of as + /// an intersection of halfspaces. Regions may be open or + /// closed in their boundaries, if any. + /// + /// Note that the Region3 class is essentially a set R ⊆ R^3. + /// For 3D solid box semantics, use the `AxisAlignedBox` class + /// instead. + /// + /// ## Example + /// + /// \snippet examples/region3_example.cc complete + template + class Region3 + { + /// \brief An unbounded region (-∞, ∞) ✕ (-∞, ∞) ✕ (-∞, ∞) + public: static const Region3 &Unbounded; + + /// \brief Constructor + public: Region3() = default; + + /// \brief Constructor + /// \param[in] _ix x-axis interval + /// \param[in] _iy y-axis interval + /// \param[in] _iz z-axis interval + public: constexpr Region3( + Interval _ix, Interval _iy, Interval _iz) + : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz)) + { + } + + /// \brief Make an open region + /// \param[in] _xLeft leftmost x-axis interval value + /// \param[in] _xRight righmost x-axis interval value + /// \param[in] _yLeft leftmost y-axis interval value + /// \param[in] _yRight righmost y-axis interval value + /// \param[in] _zLeft leftmost z-axis interval value + /// \param[in] _zRight righmost z-axis interval value + /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`) + /// ✕ (`_zLeft`, `_zRight`) open region + public: static constexpr Region3 Open( + T _xLeft, T _yLeft, T _zLeft, + T _xRight, T _yRight, T _zRight) + { + return Region3(Interval::Open(_xLeft, _xRight), + Interval::Open(_yLeft, _yRight), + Interval::Open(_zLeft, _zRight)); + } + + /// \brief Make a closed region + /// \param[in] _xLeft leftmost x-axis interval value + /// \param[in] _xRight righmost x-axis interval value + /// \param[in] _yLeft leftmost y-axis interval value + /// \param[in] _yRight righmost y-axis interval value + /// \param[in] _zLeft leftmost z-axis interval value + /// \param[in] _zRight righmost z-axis interval value + /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`] + /// ✕ [`_zLeft`, `_zRight`] closed region + public: static constexpr Region3 Closed( + T _xLeft, T _yLeft, T _zLeft, + T _xRight, T _yRight, T _zRight) + { + return Region3(Interval::Closed(_xLeft, _xRight), + Interval::Closed(_yLeft, _yRight), + Interval::Closed(_zLeft, _zRight)); + } + + /// \brief Get the x-axis interval for the region + /// \return the x-axis interval + public: const Interval &Ix() const { return this->ix; } + + /// \brief Get the y-axis interval for the region + /// \return the y-axis interval + public: const Interval &Iy() const { return this->iy; } + + /// \brief Get the z-axis interval for the region + /// \return the z-axis interval + public: const Interval &Iz() const { return this->iz; } + + /// \brief Check if the region is empty + /// A region is empty if any of the intervals + /// it is defined with (i.e. Ix, Iy, Iz) are. + /// \return true if it is empty, false otherwise + public: bool Empty() const + { + return this->ix.Empty() || this->iy.Empty() || this->iz.Empty(); + } + + /// \brief Check if the region contains `_point` + /// \param[in] _point point to check for membership + /// \return true if it is contained, false otherwise + public: bool Contains(const Vector3 &_point) const + { + return (this->ix.Contains(_point.X()) && + this->iy.Contains(_point.Y()) && + this->iz.Contains(_point.Z())); + } + + /// \brief Check if the region contains `_other` region + /// \param[in] _other region to check for membership + /// \return true if it is contained, false otherwise + public: bool Contains(const Region3 &_other) const + { + return (this->ix.Contains(_other.ix) && + this->iy.Contains(_other.iy) && + this->iz.Contains(_other.iz)); + } + + /// \brief Check if the region intersects `_other` region + /// \param[in] _other region to check for intersection + /// \return true if it is contained, false otherwise + public: bool Intersects(const Region3& _other) const + { + return (this->ix.Intersects(_other.ix) && + this->iy.Intersects(_other.iy) && + this->iz.Intersects(_other.iz)); + } + + /// \brief Equality test operator + /// \param _other region to check for equality + /// \return true if regions are equal, false otherwise + public: bool operator==(const Region3 &_other) const + { + return this->Contains(_other) && _other.Contains(*this); + } + + /// \brief Inequality test operator + /// \param _other region to check for inequality + /// \return true if regions are unequal, false otherwise + public: bool operator!=(const Region3 &_other) const + { + return !this->Contains(_other) || !_other.Contains(*this); + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _r Region3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Region3 &_r) + { + return _out <<_r.ix << " x " << _r.iy << " x " << _r.iz; + } + + /// \brief The x-axis interval + private: Interval ix; + /// \brief The y-axis interval + private: Interval iy; + /// \brief The z-axis interval + private: Interval iz; + }; + + namespace detail { + template + constexpr Region3 gUnboundedRegion3( + Interval::Open(-std::numeric_limits::infinity(), + std::numeric_limits::infinity()), + Interval::Open(-std::numeric_limits::infinity(), + std::numeric_limits::infinity()), + Interval::Open(-std::numeric_limits::infinity(), + std::numeric_limits::infinity())); + } + template + const Region3 &Region3::Unbounded = detail::gUnboundedRegion3; + + using Region3f = Region3; + using Region3d = Region3; + } + } +} + +#endif diff --git a/include/gz/math/RollingMean.hh b/include/gz/math/RollingMean.hh new file mode 100644 index 000000000..f56f07524 --- /dev/null +++ b/include/gz/math/RollingMean.hh @@ -0,0 +1,90 @@ +/* + * 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. + * +*/ +#ifndef GZ_MATH_ROLLINGMEAN_HH_ +#define GZ_MATH_ROLLINGMEAN_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declarations. + class RollingMeanPrivate; + + /// \brief A class that computes the mean over a series of data points. + /// The window size determines the maximum number of data points. The + /// oldest value is popped off when the window size is reached and + /// a new value is pushed in. + class IGNITION_MATH_VISIBLE RollingMean + { + /// \brief Constructor + /// \param[in] _windowSize The window size to use. This value will be + /// ignored if it is equal to zero. + public: explicit RollingMean(size_t _windowSize = 10); + + /// \brief Destructor. + public: ~RollingMean(); + + /// \brief Get the mean value. + /// \return The current mean value, or + /// std::numeric_limits::quiet_NaN() if data points are not + /// present. + public: double Mean() const; + + /// \brief Get the number of data points. + /// \return The number of datapoints. + public: size_t Count() const; + + /// \brief Insert a new value. + /// \param[in] _value New value to insert. + public: void Push(double _value); + + /// \brief Remove all the pushed values. + public: void Clear(); + + /// \brief Set the new window size. This will also clear the data. + /// Nothing happens if the _windowSize is zero. + /// \param[in] _windowSize The window size to use. + public: void SetWindowSize(size_t _windowSize); + + /// \brief Get the window size. + /// \return The window size. + public: size_t WindowSize() const; + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} + +#endif diff --git a/include/gz/math/RotationSpline.hh b/include/gz/math/RotationSpline.hh new file mode 100644 index 000000000..c89609510 --- /dev/null +++ b/include/gz/math/RotationSpline.hh @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2012 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_ROTATIONSPLINE_HH_ +#define GZ_MATH_ROTATIONSPLINE_HH_ + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private data + class RotationSplinePrivate; + + /// \class RotationSpline RotationSpline.hh ignition/math/RotationSpline.hh + /// \brief Spline for rotations + class IGNITION_MATH_VISIBLE RotationSpline + { + /// \brief Constructor. Sets the autoCalc to true + public: RotationSpline(); + + /// \brief Destructor. Nothing is done + public: ~RotationSpline(); + + /// \brief Adds a control point to the end of the spline. + /// \param[in] _p control point + public: void AddPoint(const Quaterniond &_p); + + /// \brief Gets the detail of one of the control points of the spline. + /// \param[in] _index the index of the control point. _index is + /// clamped to [0, PointCount()-1]. + /// \remarks This point must already exist in the spline. + /// \return The quaternion at the specified point. + /// If there are no points, then a Quaterniond with a value of + /// [INF, INF, INF, INF] is returned. + public: const Quaterniond &Point(const unsigned int _index) const; + + /// \brief Gets the number of control points in the spline. + /// \return the count + public: unsigned int PointCount() const; + + /// \brief Clears all the points in the spline. + public: void Clear(); + + /// \brief Updates a single point in the spline. + /// \remarks This point must already exist in the spline. + /// \param[in] _index index + /// \param[in] _value the new control point value + /// \return True on success, false if _index is larger or equal than + /// PointCount(). + public: bool UpdatePoint(const unsigned int _index, + const Quaterniond &_value); + + /// \brief Returns an interpolated point based on a parametric + /// value over the whole series. + /// \remarks Given a t value between 0 and 1 representing the + /// parametric distance along the whole length of the spline, + /// this method returns an interpolated point. + /// \param[in] _t Parametric value. + /// \param[in] _useShortestPath Defines if rotation should take the + /// shortest possible path + /// \return The rotation, or [INF, INF, INF, INF] on error. Use + /// Quateriond::IsFinite() to check for an error + public: Quaterniond Interpolate(double _t, + const bool _useShortestPath = true); + + /// \brief Interpolates a single segment of the spline + /// given a parametric value. + /// \param[in] _fromIndex The point index to treat as t = 0. + /// _fromIndex + 1 is deemed to be t = 1 + /// \param[in] _t Parametric value + /// \param[in] _useShortestPath Defines if rotation should take the + /// shortest possible path + /// \return the rotation, or [INF, INF, INF, INF] on error. Use + /// Quateriond::IsFinite() to check for an error + public: Quaterniond Interpolate(const unsigned int _fromIndex, + const double _t, const bool _useShortestPath = true); + + /// \brief Tells the spline whether it should automatically calculate + /// tangents on demand as points are added. + /// \remarks The spline calculates tangents at each point automatically + /// based on the input points. Normally it does this every + /// time a point changes. However, if you have a lot of points + /// to add in one go, you probably don't want to incur this + /// overhead and would prefer to defer the calculation until + /// you are finished setting all the points. You can do this + /// by calling this method with a parameter of 'false'. Just + /// remember to manually call the recalcTangents method when + /// you are done. + /// \param[in] _autoCalc If true, tangents are calculated for you + /// whenever a point changes. If false, you must call reclacTangents to + /// recalculate them when it best suits. + public: void AutoCalculate(bool _autoCalc); + + /// \brief Recalculates the tangents associated with this spline. + /// \remarks If you tell the spline not to update on demand by calling + /// setAutoCalculate(false) then you must call this after + /// completing your updates to the spline points. + public: void RecalcTangents(); + + /// \brief Private data pointer + private: RotationSplinePrivate *dataPtr; + }; + } + } +} + +#endif diff --git a/include/gz/math/SemanticVersion.hh b/include/gz/math/SemanticVersion.hh new file mode 100644 index 000000000..136ce853b --- /dev/null +++ b/include/gz/math/SemanticVersion.hh @@ -0,0 +1,164 @@ +/* + * Copyright (C) 2016 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_SEMANTICVERSION_HH_ +#define GZ_MATH_SEMANTICVERSION_HH_ + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private data class + class SemanticVersionPrivate; + + /// \class SemanticVersion SemanticVersion.hh + /// ignition/math/SemanticVersion.hh + /// \brief Version comparison class based on Semantic Versioning 2.0.0 + /// http://semver.org/ + /// Compares versions and converts versions from string. + class IGNITION_MATH_VISIBLE SemanticVersion + { + /// \brief Default constructor. Use the Parse function to populate + /// an instance with version information. + public: SemanticVersion(); + + /// \brief Constructor + /// \param[in] _v the string version. ex: "0.3.2" + public: explicit SemanticVersion(const std::string &_v); + + /// \brief Copy constructor + /// \param[in] _copy the other version + public: SemanticVersion(const SemanticVersion &_copy); + + /// \brief Assignment operator + /// \param[in] _other The version to assign from. + /// \return The reference to this instance + public: SemanticVersion &operator=(const SemanticVersion &_other); + + /// \brief Constructor + /// \param[in] _major The major number + /// \param[in] _minor The minor number + /// \param[in] _patch The patch number + /// \param[in] _prerelease The prerelease string + /// \param[in] _build The build metadata string + public: SemanticVersion(const unsigned int _major, + const unsigned int _minor = 0, + const unsigned int _patch = 0, + const std::string &_prerelease = "", + const std::string &_build = ""); + + /// \brief Destructor + public: ~SemanticVersion(); + + /// \brief Parse a version string and set the major, minor, patch + /// numbers, and prerelease and build strings. + /// \param[in] _versionStr The version string, such as "1.2.3-pr+123" + /// \return True on success. + public: bool Parse(const std::string &_versionStr); + + /// \brief Returns the version as a string + /// \return The semantic version string + public: std::string Version() const; + + /// \brief Get the major number + /// \return The major number + public: unsigned int Major() const; + + /// \brief Get the minor number + /// \return The minor number + public: unsigned int Minor() const; + + /// \brief Get the patch number + /// \return The patch number + public: unsigned int Patch() const; + + /// \brief Get the prerelease string. + /// \return Prelrease string, empty if a prerelease string was not + /// specified. + public: std::string Prerelease() const; + + /// \brief Get the build metadata string. Build meta data is not used + /// when determining precedence. + /// \return Build metadata string, empty if a build metadata string was + /// not specified. + public: std::string Build() const; + + /// \brief Less than comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is newer + public: bool operator<(const SemanticVersion &_other) const; + + /// \brief Less than or equal comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is newer or equal + public: bool operator<=(const SemanticVersion &_other) const; + + /// \brief Greater than comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is older + public: bool operator>(const SemanticVersion &_other) const; + + /// \brief Greater than or equal comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is older or the same + public: bool operator>=(const SemanticVersion &_other) const; + + /// \brief Equality comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is the same + public: bool operator==(const SemanticVersion &_other) const; + + /// \brief Inequality comparison operator + /// \param[in] _other The other version to compare to + /// \return True if _other version is different + public: bool operator!=(const SemanticVersion &_other) const; + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _v Semantic version to output + /// \return the stream + public: friend std::ostream &operator<<(std::ostream &_out, + const SemanticVersion &_v) + { + _out << _v.Version(); + return _out; + } + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif diff --git a/include/gz/math/SignalStats.hh b/include/gz/math/SignalStats.hh new file mode 100644 index 000000000..360800bec --- /dev/null +++ b/include/gz/math/SignalStats.hh @@ -0,0 +1,255 @@ +/* + * Copyright (C) 2015 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_SIGNALSTATS_HH_ +#define GZ_MATH_SIGNALSTATS_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \brief Forward declare private data class. + class SignalStatisticPrivate; + + /// \class SignalStatistic SignalStats.hh ignition/math/SignalStats.hh + /// \brief Statistical properties of a discrete time scalar signal. + class IGNITION_MATH_VISIBLE SignalStatistic + { + /// \brief Constructor + public: SignalStatistic(); + + /// \brief Destructor + public: virtual ~SignalStatistic(); + + /// \brief Copy constructor + /// \param[in] _ss SignalStatistic to copy + public: SignalStatistic(const SignalStatistic &_ss); + + /// \brief Get the current value of the statistical measure. + /// \return Current value of the statistical measure. + public: virtual double Value() const = 0; + + /// \brief Get a short version of the name of this statistical measure. + /// \return Short name of the statistical measure. + public: virtual std::string ShortName() const = 0; + + /// \brief Get number of data points in measurement. + /// \return Number of data points in measurement. + public: virtual size_t Count() const; + + /// \brief Add a new sample to the statistical measure. + /// \param[in] _data New signal data point. + public: virtual void InsertData(const double _data) = 0; + + /// \brief Forget all previous data. + public: virtual void Reset(); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Pointer to private data. + protected: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + + /// \class SignalMaximum SignalStats.hh ignition/math/SignalStats.hh + /// \brief Computing the maximum value of a discretely sampled signal. + class IGNITION_MATH_VISIBLE SignalMaximum : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "max" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \class SignalMean SignalStats.hh ignition/math/SignalStats.hh + /// \brief Computing the mean value of a discretely sampled signal. + class IGNITION_MATH_VISIBLE SignalMean : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "mean" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \class SignalMinimum SignalStats.hh ignition/math/SignalStats.hh + /// \brief Computing the minimum value of a discretely sampled signal. + class IGNITION_MATH_VISIBLE SignalMinimum : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "min" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \class SignalRootMeanSquare SignalStats.hh ignition/math/SignalStats.hh + /// \brief Computing the square root of the mean squared value + /// of a discretely sampled signal. + class IGNITION_MATH_VISIBLE SignalRootMeanSquare : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "rms" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \class SignalMaxAbsoluteValue SignalStats.hh + /// ignition/math/SignalStats.hh + /// \brief Computing the maximum of the absolute value + /// of a discretely sampled signal. + /// Also known as the maximum norm, infinity norm, or supremum norm. + class IGNITION_MATH_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "maxAbs" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \class SignalVariance SignalStats.hh ignition/math/SignalStats.hh + /// \brief Computing the incremental variance + /// of a discretely sampled signal. + class IGNITION_MATH_VISIBLE SignalVariance : public SignalStatistic + { + // Documentation inherited. + public: virtual double Value() const override; + + /// \brief Get a short version of the name of this statistical measure. + /// \return "var" + public: virtual std::string ShortName() const override; + + // Documentation inherited. + public: virtual void InsertData(const double _data) override; + }; + + /// \brief Forward declare private data class. + class SignalStatsPrivate; + + /// \class SignalStats SignalStats.hh ignition/math/SignalStats.hh + /// \brief Collection of statistics for a scalar signal. + class IGNITION_MATH_VISIBLE SignalStats + { + /// \brief Constructor + public: SignalStats(); + + /// \brief Destructor + public: ~SignalStats(); + + /// \brief Copy constructor + /// \param[in] _ss SignalStats to copy + public: SignalStats(const SignalStats &_ss); + + /// \brief Get number of data points in first statistic. + /// Technically you can have different numbers of data points + /// in each statistic if you call InsertStatistic after InsertData, + /// but this is not a recommended use case. + /// \return Number of data points in first statistic. + public: size_t Count() const; + + /// \brief Get the current values of each statistical measure, + /// stored in a map using the short name as the key. + /// \return Map with short name of each statistic as key + /// and value of statistic as the value. + public: std::map Map() const; + + /// \brief Add a new sample to the statistical measures. + /// \param[in] _data New signal data point. + public: void InsertData(const double _data); + + /// \brief Add a new type of statistic. + /// \param[in] _name Short name of new statistic. + /// Valid values include: + /// "maxAbs" + /// "mean" + /// "rms" + /// \return True if statistic was successfully added, + /// false if name was not recognized or had already + /// been inserted. + public: bool InsertStatistic(const std::string &_name); + + /// \brief Add multiple statistics. + /// \param[in] _names Comma-separated list of new statistics. + /// For example, all statistics could be added with: + /// "maxAbs,mean,rms" + /// \return True if all statistics were successfully added, + /// false if any names were not recognized or had already + /// been inserted. + public: bool InsertStatistics(const std::string &_names); + + /// \brief Forget all previous data. + public: void Reset(); + + /// \brief Assignment operator + /// \param[in] _s A SignalStats to copy + /// \return this + public: SignalStats &operator=(const SignalStats &_s); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif + diff --git a/include/gz/math/SpeedLimiter.hh b/include/gz/math/SpeedLimiter.hh new file mode 100644 index 000000000..e90ce8f20 --- /dev/null +++ b/include/gz/math/SpeedLimiter.hh @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2021 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_SYSTEMS_SPEEDLIMITER_HH_ +#define GZ_MATH_SYSTEMS_SPEEDLIMITER_HH_ + +#include +#include +#include +#include "gz/math/Helpers.hh" + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // Forward declaration. + class SpeedLimiterPrivate; + + /// \brief Class to limit velocity, acceleration and jerk. + class IGNITION_MATH_VISIBLE SpeedLimiter + { + /// \brief Constructor. + /// There are no limits by default. + public: SpeedLimiter(); + + /// \brief Destructor. + public: ~SpeedLimiter(); + + /// \brief Set minimum velocity limit in m/s, usually <= 0. + /// \param[in] _lim Minimum velocity. + public: void SetMinVelocity(double _lim); + + /// \brief Get minimum velocity limit, defaults to negative infinity. + /// \return Minimum velocity. + public: double MinVelocity() const; + + /// \brief Set maximum velocity limit in m/s, usually >= 0. + /// \param[in] _lim Maximum velocity. + public: void SetMaxVelocity(double _lim); + + /// \brief Get maximum velocity limit, defaults to positive infinity. + /// \return Maximum velocity. + public: double MaxVelocity() const; + + /// \brief Set minimum acceleration limit in m/s^2, usually <= 0. + /// \param[in] _lim Minimum acceleration. + public: void SetMinAcceleration(double _lim); + + /// \brief Get minimum acceleration limit, defaults to negative infinity. + /// \return Minimum acceleration. + public: double MinAcceleration() const; + + /// \brief Set maximum acceleration limit in m/s^2, usually >= 0. + /// \param[in] _lim Maximum acceleration. + public: void SetMaxAcceleration(double _lim); + + /// \brief Get maximum acceleration limit, defaults to positive infinity. + /// \return Maximum acceleration. + public: double MaxAcceleration() const; + + /// \brief Set minimum jerk limit in m/s^3, usually <= 0. + /// \param[in] _lim Minimum jerk. + public: void SetMinJerk(double _lim); + + /// \brief Get minimum jerk limit, defaults to negative infinity. + /// \return Minimum jerk. + public: double MinJerk() const; + + /// \brief Set maximum jerk limit in m/s^3, usually >= 0. + /// \param[in] _lim Maximum jerk. + public: void SetMaxJerk(double _lim); + + /// \brief Get maximum jerk limit, defaults to positive infinity. + /// \return Maximum jerk. + public: double MaxJerk() const; + + /// \brief Limit velocity, acceleration and jerk. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \param [in] _prevVel Previous velocity to _vel [m/s]. + /// \param [in] _prevPrevVel Previous velocity to _prevVel [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double Limit(double &_vel, + double _prevVel, + double _prevPrevVel, + std::chrono::steady_clock::duration _dt) const; + + /// \brief Limit the velocity. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double LimitVelocity(double &_vel) const; + + /// \brief Limit the acceleration using a first-order backward difference + /// method. + /// \param [in, out] _vel Velocity [m/s]. + /// \param [in] _prevVel Previous velocity [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + public: double LimitAcceleration( + double &_vel, + double _prevVel, + std::chrono::steady_clock::duration _dt) const; + + /// \brief Limit the jerk using a second-order backward difference method. + /// \param [in, out] _vel Velocity to limit [m/s]. + /// \param [in] _prevVel Previous velocity to v [m/s]. + /// \param [in] _prevPrevVel Previous velocity to prevVel [m/s]. + /// \param [in] _dt Time step. + /// \return Limiting difference, which is (out _vel - in _vel). + /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. + public: double LimitJerk( + double &_vel, + double _prevVel, + double _prevPrevVel, + std::chrono::steady_clock::duration _dt) const; + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } +} +} + +#endif diff --git a/include/gz/math/Sphere.hh b/include/gz/math/Sphere.hh new file mode 100644 index 000000000..8da5a82d1 --- /dev/null +++ b/include/gz/math/Sphere.hh @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2018 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_SPHERE_HH_ +#define GZ_MATH_SPHERE_HH_ + +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Plane.hh" + +namespace ignition +{ + namespace math + { + // Foward declarations + class SpherePrivate; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Sphere Sphere.hh ignition/math/Sphere.hh + /// \brief A representation of a sphere. + /// + /// The sphere class supports defining a sphere with a radius and + /// material properties. Radius is in meters. + /// See Material for more on material properties. + template + class Sphere + { + /// \brief Default constructor. The default radius is zero. + public: Sphere() = default; + + /// \brief Construct a sphere with a radius. + /// \param[in] _radius Radius of the sphere. + public: explicit Sphere(const Precision _radius); + + /// \brief Construct a sphere with a radius, material + /// \param[in] _radius Radius of the sphere. + /// \param[in] _mat Material property for the sphere. + public: Sphere(const Precision _radius, const Material &_mat); + + /// \brief Destructor + public: ~Sphere() = default; + + /// \brief Get the radius in meters. + /// \return The radius of the sphere in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the sphere in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the material associated with this sphere. + /// \return The material assigned to this sphere + public: const gz::math::Material &Material() const; + + /// \brief Set the material associated with this sphere. + /// \param[in] _mat The material assigned to this sphere + public: void SetMaterial(const gz::math::Material &_mat); + + /// \brief Get the mass matrix for this sphere. This function + /// is only meaningful if the sphere's radius and material have been set. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid radius (<=0) or density (<=0). + public: bool MassMatrix(MassMatrix3d &_massMat) const; + + /// \brief Check if this sphere is equal to the provided sphere. + /// Radius and material properties will be checked. + public: bool operator==(const Sphere &_sphere) const; + + /// \brief Check if this sphere is not equal to the provided sphere. + /// Radius and material properties will be checked. + public: bool operator!=(const Sphere &_sphere) const; + + /// \brief Get the volume of the sphere in m^3. + /// \return Volume of the sphere in m^3. + public: Precision Volume() const; + + /// \brief Get the volume of sphere below a given plane in m^3. + /// It is assumed that the center of the sphere is on the origin + /// \param[in] _plane The plane which slices this sphere, expressed + /// in the sphere's reference frame. + /// \return Volume below the sphere in m^3. + public: Precision VolumeBelow(const Plane &_plane) const; + + /// \brief Center of volume below the plane. This is useful for example + /// when calculating where buoyancy should be applied. + /// \param[in] _plane The plane which slices this sphere, expressed + /// in the sphere's reference frame. + /// \return The center of volume if there is anything under the plane, + /// otherwise return a std::nullopt. Expressed in the sphere's reference + /// frame. + public: std::optional> + CenterOfVolumeBelow(const Plane &_plane) const; + + /// \brief Compute the sphere's density given a mass value. The + /// sphere is assumed to be solid with uniform density. This + /// function requires the sphere's radius to be set to a + /// value greater than zero. The Material of the sphere is ignored. + /// \param[in] _mass Mass of the sphere, in kg. This value should be + /// greater than zero. + /// \return Density of the sphere in kg/m^3. A negative value is + /// returned if radius or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this sphere based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// sphere is assumed to be solid with uniform density. This + /// function requires the sphere's radius to be set to a + /// value greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the sphere, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// sphere's radius or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the sphere. + private: Precision radius = 0.0; + + /// \brief the sphere's material. + private: gz::math::Material material; + }; + + /// \typedef Sphere Spherei + /// \brief Sphere with integer precision. + typedef Sphere Spherei; + + /// \typedef Sphere Sphered + /// \brief Sphere with double precision. + typedef Sphere Sphered; + + /// \typedef Sphere Spheref + /// \brief Sphere with float precision. + typedef Sphere Spheref; + } + } +} +#include "gz/math/detail/Sphere.hh" + +#endif diff --git a/include/gz/math/SphericalCoordinates.hh b/include/gz/math/SphericalCoordinates.hh new file mode 100644 index 000000000..ecdbf4602 --- /dev/null +++ b/include/gz/math/SphericalCoordinates.hh @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2012 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_SPHERICALCOORDINATES_HH_ +#define GZ_MATH_SPHERICALCOORDINATES_HH_ + +#include +#include + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + class SphericalCoordinatesPrivate; + + /// \brief Convert spherical coordinates for planetary surfaces. + class IGNITION_MATH_VISIBLE SphericalCoordinates + { + /// \enum SurfaceType + /// \brief Unique identifiers for planetary surface models. + public: enum SurfaceType + { + /// \brief Model of reference ellipsoid for earth, based on + /// WGS 84 standard. see wikipedia: World_Geodetic_System + EARTH_WGS84 = 1 + }; + + /// \enum CoordinateType + /// \brief Unique identifiers for coordinate types. + public: enum CoordinateType + { + /// \brief Latitude, Longitude and Altitude by SurfaceType + SPHERICAL = 1, + + /// \brief Earth centered, earth fixed Cartesian + ECEF = 2, + + /// \brief Local tangent plane (East, North, Up) + GLOBAL = 3, + + /// \brief Heading-adjusted tangent plane (X, Y, Z) + /// This has kept a bug for backwards compatibility, use + /// LOCAL2 for the correct behaviour. + LOCAL = 4, + + /// \brief Heading-adjusted tangent plane (X, Y, Z) + LOCAL2 = 5 + }; + + /// \brief Constructor. + public: SphericalCoordinates(); + + /// \brief Constructor with surface type input. + /// \param[in] _type SurfaceType specification. + public: explicit SphericalCoordinates(const SurfaceType _type); + + /// \brief Constructor with surface type, angle, and elevation inputs. + /// \param[in] _type SurfaceType specification. + /// \param[in] _latitude Reference latitude. + /// \param[in] _longitude Reference longitude. + /// \param[in] _elevation Reference elevation. + /// \param[in] _heading Heading offset. + public: SphericalCoordinates(const SurfaceType _type, + const gz::math::Angle &_latitude, + const gz::math::Angle &_longitude, + const double _elevation, + const gz::math::Angle &_heading); + + /// \brief Copy constructor. + /// \param[in] _sc Spherical coordinates to copy. + public: SphericalCoordinates(const SphericalCoordinates &_sc); + + /// \brief Destructor. + public: ~SphericalCoordinates(); + + /// \brief Convert a Cartesian position vector to geodetic coordinates. + /// This performs a `PositionTransform` from LOCAL to SPHERICAL. + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results. + /// Note that `PositionTransform` returns spherical coordinates in + /// radians. + /// + /// \param[in] _xyz Cartesian position vector in the heading-adjusted + /// world frame. + /// \return Cooordinates: geodetic latitude (deg), longitude (deg), + /// altitude above sea level (m). + public: gz::math::Vector3d SphericalFromLocalPosition( + const gz::math::Vector3d &_xyz) const; + + /// \brief Convert a Cartesian velocity vector in the local frame + /// to a global Cartesian frame with components East, North, Up. + /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results. + /// + /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted + /// world frame. + /// \return Rotated vector with components (x,y,z): (East, North, Up). + public: gz::math::Vector3d GlobalFromLocalVelocity( + const gz::math::Vector3d &_xyz) const; + + /// \brief Convert a string to a SurfaceType. + /// Allowed values: ["EARTH_WGS84"]. + /// \param[in] _str String to convert. + /// \return Conversion to SurfaceType. + public: static SurfaceType Convert(const std::string &_str); + + /// \brief Convert a SurfaceType to a string. + /// \param[in] _type Surface type + /// \return Type as string + public: static std::string Convert(SurfaceType _type); + + /// \brief Get the distance between two points expressed in geographic + /// latitude and longitude. It assumes that both points are at sea level. + /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents + /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W. + /// \param[in] _latA Latitude of point A. + /// \param[in] _lonA Longitude of point A. + /// \param[in] _latB Latitude of point B. + /// \param[in] _lonB Longitude of point B. + /// \return Distance in meters. + public: static double Distance(const gz::math::Angle &_latA, + const gz::math::Angle &_lonA, + const gz::math::Angle &_latB, + const gz::math::Angle &_lonB); + + /// \brief Get SurfaceType currently in use. + /// \return Current SurfaceType value. + public: SurfaceType Surface() const; + + /// \brief Get reference geodetic latitude. + /// \return Reference geodetic latitude. + public: gz::math::Angle LatitudeReference() const; + + /// \brief Get reference longitude. + /// \return Reference longitude. + public: gz::math::Angle LongitudeReference() const; + + /// \brief Get reference elevation in meters. + /// \return Reference elevation. + public: double ElevationReference() const; + + /// \brief Get heading offset for the reference frame, expressed as + /// angle from East to x-axis, or equivalently + /// from North to y-axis. + /// \return Heading offset of reference frame. + public: gz::math::Angle HeadingOffset() const; + + /// \brief Set SurfaceType for planetary surface model. + /// \param[in] _type SurfaceType value. + public: void SetSurface(const SurfaceType &_type); + + /// \brief Set reference geodetic latitude. + /// \param[in] _angle Reference geodetic latitude. + public: void SetLatitudeReference(const gz::math::Angle &_angle); + + /// \brief Set reference longitude. + /// \param[in] _angle Reference longitude. + public: void SetLongitudeReference(const gz::math::Angle &_angle); + + /// \brief Set reference elevation above sea level in meters. + /// \param[in] _elevation Reference elevation. + public: void SetElevationReference(const double _elevation); + + /// \brief Set heading angle offset for the frame. + /// \param[in] _angle Heading offset for the frame. + public: void SetHeadingOffset(const gz::math::Angle &_angle); + + /// \brief Convert a geodetic position vector to Cartesian coordinates. + /// This performs a `PositionTransform` from SPHERICAL to LOCAL. + /// \param[in] _latLonEle Geodetic position in the planetary frame of + /// reference. X: latitude (deg), Y: longitude (deg), X: altitude. + /// \return Cartesian position vector in the heading-adjusted world frame. + public: gz::math::Vector3d LocalFromSphericalPosition( + const gz::math::Vector3d &_latLonEle) const; + + /// \brief Convert a Cartesian velocity vector with components East, + /// North, Up to a local cartesian frame vector XYZ. + /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` + /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up). + /// \return Cartesian vector in the world frame. + public: gz::math::Vector3d LocalFromGlobalVelocity( + const gz::math::Vector3d &_xyz) const; + + /// \brief Update coordinate transformation matrix with reference location + public: void UpdateTransformationMatrix(); + + /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. + /// \param[in] _pos Position vector in frame defined by parameter _in + /// \param[in] _in CoordinateType for input + /// \param[in] _out CoordinateType for output + /// \return Transformed coordinate using cached origin. + public: gz::math::Vector3d + PositionTransform(const gz::math::Vector3d &_pos, + const CoordinateType &_in, const CoordinateType &_out) const; + + /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. + /// \param[in] _vel Velocity vector in frame defined by parameter _in + /// \param[in] _in CoordinateType for input + /// \param[in] _out CoordinateType for output + /// \return Transformed velocity vector + public: gz::math::Vector3d VelocityTransform( + const gz::math::Vector3d &_vel, + const CoordinateType &_in, const CoordinateType &_out) const; + + /// \brief Equality operator, result = this == _sc + /// \param[in] _sc Spherical coordinates to check for equality + /// \return true if this == _sc + public: bool operator==(const SphericalCoordinates &_sc) const; + + /// \brief Inequality + /// \param[in] _sc Spherical coordinates to check for inequality + /// \return true if this != _sc + public: bool operator!=(const SphericalCoordinates &_sc) const; + + /// \brief Assignment operator + /// \param[in] _sc The spherical coordinates to copy from. + /// \return this + public: SphericalCoordinates &operator=( + const SphericalCoordinates &_sc); + + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \internal + /// \brief Pointer to the private data + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif diff --git a/include/gz/math/Spline.hh b/include/gz/math/Spline.hh new file mode 100644 index 000000000..353aba3fe --- /dev/null +++ b/include/gz/math/Spline.hh @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2012 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. + * +*/ +// Note: Originally cribbed from Ogre3d. Modified to implement Cardinal +// spline and catmull-rom spline +#ifndef GZ_MATH_SPLINE_HH_ +#define GZ_MATH_SPLINE_HH_ + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private classes + class ControlPoint; + class SplinePrivate; + + /// \class Spline Spline.hh ignition/math/Spline.hh + /// \brief Splines + class IGNITION_MATH_VISIBLE Spline + { + /// \brief constructor + public: Spline(); + + /// \brief destructor + public: ~Spline(); + + /// \brief Sets the tension parameter. + /// \remarks A value of 0 results in a Catmull-Rom + /// spline. + /// \param[in] _t Tension value between 0.0 and 1.0 + public: void Tension(double _t); + + /// \brief Gets the tension value. + /// \return the value of the tension, which is between 0.0 and 1.0. + public: double Tension() const; + + /// \brief Gets spline arc length. + /// \return arc length or INF on error. + public: double ArcLength() const; + + /// \brief Sets spline arc length up to + /// a given parameter value \p _t. + /// \param[in] _t parameter value (range 0 to 1). + /// \return arc length up to \p _t or INF on error. + public: double ArcLength(const double _t) const; + + /// \brief Sets a spline segment arc length. + /// \param[in] _index of the spline segment. + /// \param[in] _t parameter value (range 0 to 1). + /// \return arc length of a given segment up to + /// \p _t or INF on error. + public: double ArcLength(const unsigned int _index, + const double _t) const; + + /// \brief Adds a single control point to the + /// end of the spline. + /// \param[in] _p control point value to add. + public: void AddPoint(const Vector3d &_p); + + /// \brief Adds a single control point to the end + /// of the spline with fixed tangent. + /// \param[in] _p control point value to add. + /// \param[in] _t tangent at \p _p. + public: void AddPoint(const Vector3d &_p, const Vector3d &_t); + + /// \brief Adds a single control point to the end + /// of the spline. + /// \param[in] _cp control point to add. + /// \param[in] _fixed whether this control point + /// should not be subject to tangent recomputation. + private: void AddPoint(const ControlPoint &_cp, const bool _fixed); + + /// \brief Gets the value for one of the control points + /// of the spline. + /// \param[in] _index the control point index. + /// \return the control point value, or [INF, INF, INF] + /// on error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d Point(const unsigned int _index) const; + + /// \brief Gets the tangent value for one of the control points + /// of the spline. + /// \param[in] _index the control point index. + /// \return the control point tangent, or [INF, INF, INF] + /// on error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d Tangent(const unsigned int _index) const; + + /// \brief Gets the mth derivative for one of the control points + /// of the spline. + /// \param[in] _index the control point index. + /// \param[in] _mth derivative order. + /// \return the control point mth derivative, or [INF, INF, INF] + /// on error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d MthDerivative(const unsigned int _index, + const unsigned int _mth) const; + + /// \brief Gets the number of control points in the spline. + /// \return the count + public: size_t PointCount() const; + + /// \brief Clears all the points in the spline. + public: void Clear(); + + /// \brief Updates a single control point value in the spline, + /// keeping its tangent. + /// \param[in] _index the control point index. + /// \param[in] _p the new control point value. + /// \return True on success. + public: bool UpdatePoint(const unsigned int _index, + const Vector3d &_p); + + /// \brief Updates a single control point in the spline, along + /// with its tangent. + /// \param[in] _index the control point index. + /// \param[in] _p the new control point value. + /// \param[in] _t the new control point tangent. + /// \return True on success. + public: bool UpdatePoint(const unsigned int _index, + const Vector3d &_p, + const Vector3d &_t); + + /// \brief Updates a single control point in the spline. + /// \param[in] _index the control point index + /// \param[in] _cp the new control point + /// \param[in] _fixed whether the new control point should not be + /// subject to tangent recomputation + /// \return True on success. + private: bool UpdatePoint(const unsigned int _index, + const ControlPoint &_cp, + const bool _fixed); + + /// \brief Interpolates a point on the spline + /// at parameter value \p _t. + /// \remarks Parameter value is normalized over the + /// whole spline arc length. Arc length is assumed + /// to be linear with the parameter. + /// \param[in] _t parameter value (range 0 to 1). + /// \return the interpolated point, or + /// [INF, INF, INF] on error. Use + /// Vector3d::IsFinite() to check for an error. + public: Vector3d Interpolate(const double _t) const; + + /// \brief Interpolates a point on a segment of the spline + /// at parameter value \p _t. + /// \remarks Parameter value is normalized over the + /// segment arc length. Arc length is assumed + /// to be linear with the parameter. + /// \param[in] _fromIndex The point index to treat as t = 0. + /// fromIndex + 1 is deemed to be t = 1. + /// \param[in] _t parameter value (range 0 to 1). + /// \return the interpolated point, or [INF, INF, INF] on + /// error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d Interpolate(const unsigned int _fromIndex, + const double _t) const; + + /// \brief Interpolates a tangent on the spline at + /// parameter value \p _t. + /// \remarks Parameter value is normalized over the + /// whole spline arc length. Arc length is assumed + /// to be linear with the parameter. + /// \param[in] _t parameter value (range 0 to 1). + /// \return the interpolated point, or [INF, INF, INF] + /// on error. Use Vector3d::IsFinte() to check for an error. + public: Vector3d InterpolateTangent(const double _t) const; + + /// \brief Interpolates the tangent on a segment of the spline + /// at parameter value \p _t. + /// \remarks Parameter value is normalized over the + /// segment arc length. Arc length is assumed + /// to be linear with the parameter. + /// \param[in] _fromIndex the point index to treat as t = 0. + /// fromIndex + 1 is deemed to be t = 1. + /// \param[in] _t parameter value (range 0 to 1). + /// \return the interpolated point, or [INF, INF, INF] on + /// error. Use Vector3d::IsFinte() to check for an error. + public: Vector3d InterpolateTangent(const unsigned int _fromIndex, + const double _t) const; + + /// \brief Interpolates the mth derivative of the spline at + /// parameter value \p _t. + /// \param[in] _mth order of curve derivative to interpolate. + /// \param[in] _1 parameter value (range 0 to 1). + /// \return the interpolated mth derivative, or [INF, INF, INF] + /// on error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d InterpolateMthDerivative(const unsigned int _mth, + const double _1) const; + + /// \brief Interpolates the mth derivative of a segment of the spline + /// at parameter value \p _t. + /// \remarks Parameter value is normalized over the segment + /// arc length. Arc length is assumed to be linear with the parameter. + /// \param[in] _fromIndex point index to treat as t = 0, fromIndex + 1 + /// is deemed to be t = 1. + /// \param[in] _mth order of curve derivative to interpolate. + /// \param[in] _s parameter value (range 0 to 1). + /// \return the interpolated mth derivative, or [INF, INF, INF] on + /// error. Use Vector3d::IsFinite() to check for an error. + public: Vector3d InterpolateMthDerivative(const unsigned int _fromIndex, + const unsigned int _mth, + const double _s) const; + + /// \brief Tells the spline whether it should automatically + /// calculate tangents on demand as points are added. + /// \remarks The spline calculates tangents at each point + /// automatically based on the input points. Normally it + /// does this every time a point changes. However, if you + /// have a lot of points to add in one go, you probably + /// don't want to incur this overhead and would prefer to + /// defer the calculation until you are finished setting all + /// the points. You can do this by calling this method with a + /// parameter of 'false'. Just remember to manually call the + /// recalcTangents method when you are done. + /// \param[in] _autoCalc If true, tangents are calculated for you whenever + /// a point changes. If false, you must call RecalcTangents to + /// recalculate them when it best suits. + public: void AutoCalculate(bool _autoCalc); + + /// \brief Recalculates the tangents associated with this spline. + /// \remarks If you tell the spline not to update on demand by + /// calling setAutoCalculate(false) then you must call this + /// after completing your updates to the spline points. + public: void RecalcTangents(); + + /// \brief Rebuilds spline segments. + private: void Rebuild(); + + /// \internal + /// \brief Maps \p _t parameter value over the whole spline + /// to the right segment (starting at point \p _index) with + /// the proper parameter value fraction \p _fraction. + /// \remarks Arc length is assumed to be linear with the parameter. + /// \param[in] _t parameter value over the whole spline (range 0 to 1). + /// \param[out] _index point index at which the segment starts. + /// \param[out] _fraction parameter value fraction for the given segment. + /// \return True on success. + private: bool MapToSegment(const double _t, + unsigned int &_index, + double &_fraction) const; + + /// \internal + /// \brief Private data pointer + private: SplinePrivate *dataPtr; + }; + } + } +} +#endif diff --git a/include/gz/math/Stopwatch.hh b/include/gz/math/Stopwatch.hh new file mode 100644 index 000000000..e1eca283c --- /dev/null +++ b/include/gz/math/Stopwatch.hh @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2018 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_STOPWATCH_HH_ +#define GZ_MATH_STOPWATCH_HH_ + +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Use a steady clock + using clock = std::chrono::steady_clock; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declarations. + class StopwatchPrivate; + + // + /// \class Stopwatch Stopwatch.hh ignition/math/Stopwatch.hh + /// \brief The Stopwatch keeps track of time spent in the run state, + /// accessed through ElapsedRunTime(), and time spent in the stop state, + /// accessed through ElapsedStopTime(). Elapsed run time starts accumulating + /// after the first call to Start(). Elapsed stop time starts + /// accumulation after Start() has been called followed by Stop(). The + /// stopwatch can be reset with the Reset() function. + /// + /// # Example usage + /// + /// ```{.cpp} + /// gz::math::Stopwatch watch; + /// watch.Start(); + /// + /// // do something... + /// + /// std::cout << "Elapsed time is " + /// << std::chrono::duration_cast( + /// timeSys.ElapsedRunTime()).count() << " ms\n"; + /// watch.Stop(); + /// ``` + class IGNITION_MATH_VISIBLE Stopwatch + { + /// \brief Constructor. + public: Stopwatch(); + + /// \brief Copy constructor + /// \param[in] _watch The stop watch to copy. + public: Stopwatch(const Stopwatch &_watch); + + /// \brief Move constructor + /// \param[in] _watch The stop watch to move. + public: Stopwatch(Stopwatch &&_watch) noexcept; + + /// \brief Destructor. + public: virtual ~Stopwatch(); + + /// \brief Start the stopwatch. + /// \param[in] _reset If true the stopwatch is reset first. + /// \return True if the the stopwatch was started. This will return + /// false if the stopwatch was already running. + public: bool Start(const bool _reset = false); + + /// \brief Get the time when the stopwatch was started. + /// \return The time when stopwatch was started, or + /// std::chrono::steady_clock::time_point::min() if the stopwatch + /// has not been started. + public: clock::time_point StartTime() const; + + /// \brief Stop the stopwatch + /// \return True if the stopwatch was stopped. This will return false + /// if the stopwatch is not running. + public: bool Stop(); + + /// \brief Get the time when the stopwatch was last stopped. + /// \return The time when stopwatch was last stopped, or + /// std::chrono::steady_clock::time_point::min() if the stopwatch + /// has never been stopped. + public: clock::time_point StopTime() const; + + /// \brief Get whether the stopwatch is running. + /// \return True if the stopwatch is running. + public: bool Running() const; + + /// \brief Reset the stopwatch. This resets the start time, stop time, + /// elapsed duration and elapsed stop duration. + public: void Reset(); + + /// \brief Get the amount of time that the stop watch has been + /// running. This is the total amount of run time, spannning all start + /// and stop calls. The Reset function or passing true to the Start + /// function will reset this value. + /// \return Total amount of elapsed run time. + public: clock::duration ElapsedRunTime() const; + + /// \brief Get the amount of time that the stop watch has been + /// stopped. This is the total amount of stop time, spannning all start + /// and stop calls. The Reset function or passing true to the Start + /// function will reset this value. + /// \return Total amount of elapsed stop time. + public: clock::duration ElapsedStopTime() const; + + /// \brief Equality operator. + /// \param[in] _watch The watch to compare. + /// \return True if this watch equals the provided watch. + public: bool operator==(const Stopwatch &_watch) const; + + /// \brief Inequality operator. + /// \param[in] _watch The watch to compare. + /// \return True if this watch does not equal the provided watch. + public: bool operator!=(const Stopwatch &_watch) const; + + /// \brief Copy assignment operator + /// \param[in] _watch The stop watch to copy. + /// \return Reference to this. + public: Stopwatch &operator=(const Stopwatch &_watch); + + /// \brief Move assignment operator + /// \param[in] _watch The stop watch to move. + /// \return Reference to this. + public: Stopwatch &operator=(Stopwatch &&_watch); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif diff --git a/include/gz/math/Temperature.hh b/include/gz/math/Temperature.hh new file mode 100644 index 000000000..e08c63321 --- /dev/null +++ b/include/gz/math/Temperature.hh @@ -0,0 +1,423 @@ +/* + * Copyright (C) 2016 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_TEMPERATURE_HH_ +#define GZ_MATH_TEMPERATURE_HH_ + +#include +#include + +#include +#include "gz/math/Helpers.hh" + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private data class. + class TemperaturePrivate; + + /// \brief A class that stores temperature information, and allows + /// conversion between different units. + /// + /// This class is mostly for convenience. It can be used to easily + /// convert between temperature units and encapsulate temperature values. + /// + /// The default unit is Kelvin. Most functions that accept a double + /// value will assume the double is Kelvin. The exceptions are a few of + /// the conversion functions, such as CelsiusToFahrenheit. Similarly, + /// most doubles that are returned will be in Kelvin. + /// + /// ## Example usage ## + /// + /// ### Convert from Kelvin to Celsius ### + /// + /// double celsius = gz::math::Temperature::KelvinToCelsius(2.5); + /// + /// ### Create and use a Temperature object ### + /// + /// gz::math::Temperature temp(123.5); + /// std::cout << "Temperature in Kelvin = " << temp << std::endl; + /// std::cout << "Temperature in Celsius = " + /// << temp.Celsius() << std::endl; + /// + /// temp += 100.0; + /// std::cout << "Temperature + 100.0 = " << temp << "K" << std::endl; + /// + /// gz::math::Temperature newTemp(temp); + /// newTemp += temp + 23.5; + /// std::cout << "Copied the temp object and added 23.5K. newTemp = " + /// << newTemp.Fahrenheit() << "F" << std::endl; + /// + class IGNITION_MATH_VISIBLE Temperature + { + /// \brief Default constructor + public: Temperature(); + + /// \brief Kelvin value constructor. This is a conversion constructor + /// \param[in] _temp Temperature in Kelvin + // cppcheck-suppress noExplicitConstructor + public: Temperature(const double _temp); + + /// \brief Copy constructor + /// \param[in] _temp Temperature object to copy. + public: Temperature(const Temperature &_temp); + + /// \brief Destructor + public: virtual ~Temperature(); + + /// \brief Convert Kelvin to Celsius + /// \param[in] _temp Temperature in Kelvin + /// \return Temperature in Celsius + public: static double KelvinToCelsius(const double _temp); + + /// \brief Convert Kelvin to Fahrenheit + /// \param[in] _temp Temperature in Kelvin + /// \return Temperature in Fahrenheit + public: static double KelvinToFahrenheit(const double _temp); + + /// \brief Convert Celsius to Fahrenheit + /// \param[in] _temp Temperature in Celsius + /// \return Temperature in Fahrenheit + public: static double CelsiusToFahrenheit(const double _temp); + + /// \brief Convert Celsius to Kelvin + /// \param[in] _temp Temperature in Celsius + /// \return Temperature in Kelvin + public: static double CelsiusToKelvin(const double _temp); + + /// \brief Convert Fahrenheit to Celsius + /// \param[in] _temp Temperature in Fahrenheit + /// \return Temperature in Celsius + public: static double FahrenheitToCelsius(const double _temp); + + /// \brief Convert Fahrenheit to Kelvin + /// \param[in] _temp Temperature in Fahrenheit + /// \return Temperature in Kelvin + public: static double FahrenheitToKelvin(const double _temp); + + /// \brief Set the temperature from a Kelvin value + /// \param[in] _temp Temperature in Kelvin + public: void SetKelvin(const double _temp); + + /// \brief Set the temperature from a Celsius value + /// \param[in] _temp Temperature in Celsius + public: void SetCelsius(const double _temp); + + /// \brief Set the temperature from a Fahrenheit value + /// \param[in] _temp Temperature in Fahrenheit + public: void SetFahrenheit(const double _temp); + + /// \brief Get the temperature in Kelvin + /// \return Temperature in Kelvin + public: double Kelvin() const; + + /// \brief Get the temperature in Celsius + /// \return Temperature in Celsius + public: double Celsius() const; + + /// \brief Get the temperature in Fahrenheit + /// \return Temperature in Fahrenheit + public: double Fahrenheit() const; + + /// \brief Accessor operator + /// \return Temperature in Kelvin + /// \sa Kelvin() + public: double operator()() const; + + /// \brief Assignment operator + /// \param[in] _temp Temperature in Kelvin + /// \return Reference to this instance + public: Temperature &operator=(const double _temp); + + /// \brief Assignment operator + /// \param[in] _temp Temperature object + /// \return Reference to this instance + public: Temperature &operator=(const Temperature &_temp); + + /// \brief Addition operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator+(const double _temp) const; + + /// \brief Addition operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator+(const Temperature &_temp) const; + + /// \brief Addition operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator+(const double _temp); + + /// \brief Addition operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator+(const Temperature &_temp); + + /// \brief Addition operator for double type. + /// \param[in] _t Temperature in kelvin + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: friend Temperature operator+(double _t, const Temperature &_temp) + { + return _t + _temp.Kelvin(); + } + + /// \brief Addition assignment operator + /// \param[in] _temp Temperature in Kelvin + /// \return Reference to this instance + public: const Temperature &operator+=(const double _temp); + + /// \brief Addition assignment operator + /// \param[in] _temp Temperature object + /// \return Reference to this instance + public: const Temperature &operator+=(const Temperature &_temp); + + /// \brief Subtraction operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator-(const double _temp); + + /// \brief Subtraction operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator-(const Temperature &_temp); + + /// \brief Subtraction operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator-(const double _temp) const; + + /// \brief Subtraction operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator-(const Temperature &_temp) const; + + /// \brief Subtraction operator for double type. + /// \param[in] _t Temperature in kelvin + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: friend Temperature operator-(double _t, const Temperature &_temp) + { + return _t - _temp.Kelvin(); + } + + /// \brief Subtraction assignment operator + /// \param[in] _temp Temperature in Kelvin + /// \return Reference to this instance + public: const Temperature &operator-=(const double _temp); + + /// \brief Subtraction assignment operator + /// \param[in] _temp Temperature object + /// \return Reference to this instance + public: const Temperature &operator-=(const Temperature &_temp); + + /// \brief Multiplication operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator*(const double _temp); + + /// \brief Multiplication operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator*(const Temperature &_temp); + + /// \brief Multiplication operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator*(const double _temp) const; + + /// \brief Multiplication operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator*(const Temperature &_temp) const; + + /// \brief Multiplication operator for double type. + /// \param[in] _t Temperature in kelvin + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: friend Temperature operator*(double _t, const Temperature &_temp) + { + return _t * _temp.Kelvin(); + } + + /// \brief Multiplication assignment operator + /// \param[in] _temp Temperature in Kelvin + /// \return Reference to this instance + public: const Temperature &operator*=(const double _temp); + + /// \brief Multiplication assignment operator + /// \param[in] _temp Temperature object + /// \return Reference to this instance + public: const Temperature &operator*=(const Temperature &_temp); + + /// \brief Division operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator/(const double _temp); + + /// \brief Division operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator/(const Temperature &_temp); + + /// \brief Division operator + /// \param[in] _temp Temperature in Kelvin + /// \return Resulting temperature + public: Temperature operator/(const double _temp) const; + + /// \brief Division operator + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: Temperature operator/(const Temperature &_temp) const; + + /// \brief Division operator for double type. + /// \param[in] _t Temperature in kelvin + /// \param[in] _temp Temperature object + /// \return Resulting temperature + public: friend Temperature operator/(double _t, const Temperature &_temp) + { + return _t / _temp.Kelvin(); + } + + /// \brief Division assignment operator + /// \param[in] _temp Temperature in Kelvin + /// \return Reference to this instance + public: const Temperature &operator/=(const double _temp); + + /// \brief Division assignment operator + /// \param[in] _temp Temperature object + /// \return Reference to this instance + public: const Temperature &operator/=(const Temperature &_temp); + + /// \brief Equal to operator + /// \param[in] _temp The temperature to compare + /// \return true if the temperatures are the same, false otherwise + public: bool operator==(const Temperature &_temp) const; + + /// \brief Equal to operator, where the value of _temp is assumed to + /// be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return true if the temperatures are the same, false otherwise + public: bool operator==(const double _temp) const; + + /// \brief Inequality to operator + /// \param[in] _temp The temperature to compare + /// \return false if the temperatures are the same, true otherwise + public: bool operator!=(const Temperature &_temp) const; + + /// \brief Inequality to operator, where the value of _temp is assumed to + /// be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return false if the temperatures are the same, true otherwise + public: bool operator!=(const double _temp) const; + + /// \brief Less than to operator + /// \param[in] _temp The temperature to compare + /// \return True if this is less than _temp. + public: bool operator<(const Temperature &_temp) const; + + /// \brief Less than operator, where the value of _temp is assumed to + /// be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return True if this is less than _temp. + public: bool operator<(const double _temp) const; + + /// \brief Less than or equal to operator + /// \param[in] _temp The temperature to compare + /// \return True if this is less than or equal _temp. + public: bool operator<=(const Temperature &_temp) const; + + /// \brief Less than or equal operator, + /// where the value of _temp is assumed to be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return True if this is less than or equal to _temp. + public: bool operator<=(const double _temp) const; + + /// \brief Greater than operator + /// \param[in] _temp The temperature to compare + /// \return True if this is greater than _temp. + public: bool operator>(const Temperature &_temp) const; + + /// \brief Greater than operator, where the value of _temp is assumed to + /// be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return True if this is greater than _temp. + public: bool operator>(const double _temp) const; + + /// \brief Greater than or equal to operator + /// \param[in] _temp The temperature to compare + /// \return True if this is greater than or equal to _temp. + public: bool operator>=(const Temperature &_temp) const; + + /// \brief Greater than equal operator, + /// where the value of _temp is assumed to be in Kelvin + /// \param[in] _temp The temperature (in Kelvin) to compare + /// \return True if this is greater than or equal to _temp. + public: bool operator>=(const double _temp) const; + + /// \brief Stream insertion operator + /// \param[in] _out the output stream + /// \param[in] _temp Temperature to write to the stream + /// \return the output stream + public: friend std::ostream &operator<<(std::ostream &_out, + const gz::math::Temperature &_temp) + { + _out << _temp.Kelvin(); + return _out; + } + + /// \brief Stream extraction operator + /// \param[in] _in the input stream + /// \param[in] _temp Temperature to read from to the stream. Assumes + /// temperature value is in Kelvin. + /// \return the input stream + public: friend std::istream &operator>>(std::istream &_in, + gz::math::Temperature &_temp) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + + double kelvin; + _in >> kelvin; + + if (!_in.fail()) + { + _temp.SetKelvin(kelvin); + } + return _in; + } + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} +#endif diff --git a/include/gz/math/Triangle.hh b/include/gz/math/Triangle.hh new file mode 100644 index 000000000..44f68d241 --- /dev/null +++ b/include/gz/math/Triangle.hh @@ -0,0 +1,247 @@ +/* + * Copyright (C) 2014 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_TRIANGLE_HH_ +#define GZ_MATH_TRIANGLE_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Triangle Triangle.hh ignition/math/Triangle.hh + /// \brief Triangle class and related functions. + template + class Triangle + { + /// \brief Default constructor + public: Triangle() = default; + + /// \brief Constructor + /// \param[in] _pt1 First point that defines the triangle. + /// \param[in] _pt2 Second point that defines the triangle. + /// \param[in] _pt3 Third point that defines the triangle. + public: Triangle(const math::Vector2 &_pt1, + const math::Vector2 &_pt2, + const math::Vector2 &_pt3) + { + this->Set(_pt1, _pt2, _pt3); + } + + /// \brief Set one vertex of the triangle. + /// \param[in] _index Index of the point to set, where + /// 0 == first vertex, 1 == second vertex, and 2 == third vertex. + /// The index is clamped to the range [0, 2]. + /// \param[in] _pt Value of the point to set. + public: void Set(const unsigned int _index, const math::Vector2 &_pt) + { + this->pts[clamp(_index, 0u, 2u)] = _pt; + } + + /// \brief Set all vertices of the triangle. + /// \param[in] _pt1 First point that defines the triangle. + /// \param[in] _pt2 Second point that defines the triangle. + /// \param[in] _pt3 Third point that defines the triangle. + public: void Set(const math::Vector2 &_pt1, + const math::Vector2 &_pt2, + const math::Vector2 &_pt3) + { + this->pts[0] = _pt1; + this->pts[1] = _pt2; + this->pts[2] = _pt3; + } + + /// \brief Get whether this triangle is valid, based on triangle + /// inequality: the sum of the lengths of any two sides must be greater + /// than the length of the remaining side. + /// \return True if the triangle inequality holds + public: bool Valid() const + { + T a = this->Side(0).Length(); + T b = this->Side(1).Length(); + T c = this->Side(2).Length(); + return (a+b) > c && (b+c) > a && (c+a) > b; + } + + /// \brief Get a line segment for one side of the triangle. + /// \param[in] _index Index of the side to retreive, where + /// 0 == Line2(pt1, pt2), + /// 1 == Line2(pt2, pt3), + /// 2 == Line2(pt3, pt1) + /// The index is clamped to the range [0, 2] + /// \return Line segment of the requested side. + public: Line2 Side(const unsigned int _index) const + { + if (_index == 0) + return Line2(this->pts[0], this->pts[1]); + else if (_index == 1) + return Line2(this->pts[1], this->pts[2]); + else + return Line2(this->pts[2], this->pts[0]); + } + + /// \brief Check if this triangle completely contains the given line + /// segment. + /// \param[in] _line Line to check. + /// \return True if the line's start and end points are both inside + /// this triangle. + public: bool Contains(const Line2 &_line) const + { + return this->Contains(_line[0]) && this->Contains(_line[1]); + } + + /// \brief Get whether this triangle contains the given point. + /// \param[in] _pt Point to check. + /// \return True if the point is inside or on the triangle. + public: bool Contains(const math::Vector2 &_pt) const + { + // Compute vectors + math::Vector2 v0 = this->pts[2] -this->pts[0]; + math::Vector2 v1 = this->pts[1] -this->pts[0]; + math::Vector2 v2 = _pt - this->pts[0]; + + // Compute dot products + double dot00 = v0.Dot(v0); + double dot01 = v0.Dot(v1); + double dot02 = v0.Dot(v2); + double dot11 = v1.Dot(v1); + double dot12 = v1.Dot(v2); + + // Compute barycentric coordinates + double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01); + double u = (dot11 * dot02 - dot01 * dot12) * invDenom; + double v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // Check if point is in triangle + return (u >= 0) && (v >= 0) && (u + v <= 1); + } + + /// \brief Get whether the given line intersects this triangle. + /// \param[in] _line Line to check. + /// \param[out] _ipt1 Return value of the first intersection point, + /// only valid if the return value of the function is true. + /// \param[out] _ipt2 Return value of the second intersection point, + /// only valid if the return value of the function is true. + /// \return True if the given line intersects this triangle. + public: bool Intersects(const Line2 &_line, + math::Vector2 &_ipt1, + math::Vector2 &_ipt2) const + { + if (this->Contains(_line)) + { + _ipt1 = _line[0]; + _ipt2 = _line[1]; + return true; + } + + Line2 line1(this->pts[0], this->pts[1]); + Line2 line2(this->pts[1], this->pts[2]); + Line2 line3(this->pts[2], this->pts[0]); + + math::Vector2 pt; + std::set > points; + + if (line1.Intersect(_line, pt)) + points.insert(pt); + + if (line2.Intersect(_line, pt)) + points.insert(pt); + + if (line3.Intersect(_line, pt)) + points.insert(pt); + + if (points.empty()) + { + return false; + } + else if (points.size() == 1) + { + auto iter = points.begin(); + + _ipt1 = *iter; + if (this->Contains(_line[0])) + _ipt2 = _line[0]; + else + { + _ipt2 = _line[1]; + } + } + else + { + auto iter = points.begin(); + _ipt1 = *(iter++); + _ipt2 = *iter; + } + + return true; + } + + /// \brief Get the length of the triangle's perimeter. + /// \return Sum of the triangle's line segments. + public: T Perimeter() const + { + return this->Side(0).Length() + this->Side(1).Length() + + this->Side(2).Length(); + } + + /// \brief Get the area of this triangle. + /// \return Triangle's area. + public: double Area() const + { + double s = this->Perimeter() / 2.0; + T a = this->Side(0).Length(); + T b = this->Side(1).Length(); + T c = this->Side(2).Length(); + + // Heron's formula + // http://en.wikipedia.org/wiki/Heron%27s_formula + return sqrt(s * (s-a) * (s-b) * (s-c)); + } + + /// \brief Get one of points that define the triangle. + /// \param[in] _index The index, where 0 == first vertex, + /// 1 == second vertex, and 2 == third vertex. + /// The index is clamped to the range [0, 2] + /// \return The point specified by _index. + public: math::Vector2 operator[](const size_t _index) const + { + return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; + } + + /// The points of the triangle + private: math::Vector2 pts[3]; + }; + + /// Integer specialization of the Triangle class. + typedef Triangle Trianglei; + + /// Double specialization of the Triangle class. + typedef Triangle Triangled; + + /// Float specialization of the Triangle class. + typedef Triangle Trianglef; + } + } +} +#endif diff --git a/include/gz/math/Triangle3.hh b/include/gz/math/Triangle3.hh new file mode 100644 index 000000000..215118dfb --- /dev/null +++ b/include/gz/math/Triangle3.hh @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2016 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_TRIANGLE3_HH_ +#define GZ_MATH_TRIANGLE3_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Triangle3 Triangle3.hh ignition/math/Triangle3.hh + /// \brief A 3-dimensional triangle and related functions. + template + class Triangle3 + { + /// \brief Default constructor + public: Triangle3() = default; + + /// \brief Constructor. + /// + /// Keep in mind that the triangle normal + /// is determined by the order of these vertices. Search + /// the internet for "triangle winding" for more information. + /// \param[in] _pt1 First point that defines the triangle. + /// \param[in] _pt2 Second point that defines the triangle. + /// \param[in] _pt3 Third point that defines the triangle. + public: Triangle3(const Vector3 &_pt1, + const Vector3 &_pt2, + const Vector3 &_pt3) + { + this->Set(_pt1, _pt2, _pt3); + } + + /// \brief Set one vertex of the triangle. + /// + /// Keep in mind that the triangle normal + /// is determined by the order of these vertices. Search + /// the internet for "triangle winding" for more information. + /// + /// \param[in] _index Index of the point to set. _index is clamped + /// to the range [0,2]. + /// \param[in] _pt Value of the point to set. + public: void Set(const unsigned int _index, const Vector3 &_pt) + { + this->pts[clamp(_index, 0u, 2u)] = _pt; + } + + /// \brief Set all vertices of the triangle. + /// + /// Keep in mind that the triangle normal + /// is determined by the order of these vertices. Search + /// the internet for "triangle winding" for more information. + /// + /// \param[in] _pt1 First point that defines the triangle. + /// \param[in] _pt2 Second point that defines the triangle. + /// \param[in] _pt3 Third point that defines the triangle. + public: void Set(const Vector3 &_pt1, + const Vector3 &_pt2, + const Vector3 &_pt3) + { + this->pts[0] = _pt1; + this->pts[1] = _pt2; + this->pts[2] = _pt3; + } + + /// \brief Get whether this triangle is valid, based on triangle + /// inequality: the sum of the lengths of any two sides must be greater + /// than the length of the remaining side. + /// \return True if the triangle inequality holds + public: bool Valid() const + { + T a = this->Side(0).Length(); + T b = this->Side(1).Length(); + T c = this->Side(2).Length(); + return (a+b) > c && (b+c) > a && (c+a) > b; + } + + /// \brief Get a line segment for one side of the triangle. + /// \param[in] _index Index of the side to retrieve, where + /// 0 == Line3(pt1, pt2), + /// 1 == Line3(pt2, pt3), + /// 2 == Line3(pt3, pt1). + /// _index is clamped to the range [0,2]. + /// \return Line segment of the requested side. + public: Line3 Side(const unsigned int _index) const + { + if (_index == 0) + return Line3(this->pts[0], this->pts[1]); + else if (_index == 1) + return Line3(this->pts[1], this->pts[2]); + else + return Line3(this->pts[2], this->pts[0]); + } + + /// \brief Check if this triangle completely contains the given line + /// segment. + /// \param[in] _line Line to check. + /// \return True if the line's start and end points are both inside + /// this triangle. + public: bool Contains(const Line3 &_line) const + { + return this->Contains(_line[0]) && this->Contains(_line[1]); + } + + /// \brief Get whether this triangle contains the given point. + /// \param[in] _pt Point to check. + /// \return True if the point is inside or on the triangle. + public: bool Contains(const Vector3 &_pt) const + { + // Make sure the point is on the same plane as the triangle + if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2])) + == Planed::NO_SIDE) + { + Vector3 v0 = this->pts[2] - this->pts[0]; + Vector3 v1 = this->pts[1] - this->pts[0]; + Vector3 v2 = _pt - this->pts[0]; + + double dot00 = v0.Dot(v0); + double dot01 = v0.Dot(v1); + double dot02 = v0.Dot(v2); + double dot11 = v1.Dot(v1); + double dot12 = v1.Dot(v2); + + // Compute barycentric coordinates + double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01); + double u = (dot11 * dot02 - dot01 * dot12) * invDenom; + double v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // Check if point is in triangle + return (u >= 0) && (v >= 0) && (u + v <= 1); + } + return false; + } + + /// \brief Get the triangle's normal vector. + /// \return The normal vector for the triangle. + public: Vector3d Normal() const + { + return math::Vector3d::Normal( + Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]), + Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]), + Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2])); + } + + /// \brief Get whether the given line intersects an edge of this triangle. + /// + /// The returned intersection point is one of: + /// + /// * If the line is coplanar with the triangle: + /// * The point on the closest edge of the triangle that the line + /// intersects. + /// OR + /// * The first point on the line, if the line is completely contained + /// * If the line is not coplanar, the point on the triangle that the + /// line intersects. + /// + /// \param[in] _line Line to check. + /// \param[out] _ipt1 Return value of the first intersection point, + /// only valid if the return value of the function is true. + /// \return True if the given line intersects this triangle. + public: bool Intersects( + const Line3 &_line, Vector3 &_ipt1) const + { + // Triangle normal + Vector3d norm = this->Normal(); + + // Ray direction to intersect with triangle + Vector3 dir = (_line[1] - _line[0]).Normalize(); + + double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2])); + + // Handle the case when the line is not co-planar with the triangle + if (!math::equal(denom, 0.0)) + { + // Distance from line start to triangle intersection + Vector3 diff = _line[0] - this->pts[0]; + double intersection = + -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom; + + // Make sure the ray intersects the triangle + if (intersection < 1.0 || intersection > _line.Length()) + return false; + + // Return point of intersection + _ipt1 = _line[0] + (dir * intersection); + + return true; + } + // Line co-planar with triangle + else + { + // If the line is completely inside the triangle + if (this->Contains(_line)) + { + _ipt1 = _line[0]; + return true; + } + // If the line intersects the first side + else if (_line.Intersect(this->Side(0), _ipt1)) + { + return true; + } + // If the line intersects the second side + else if (_line.Intersect(this->Side(1), _ipt1)) + { + return true; + } + // If the line intersects the third side + else if (_line.Intersect(this->Side(2), _ipt1)) + { + return true; + } + } + + return false; + } + + /// \brief Get the length of the triangle's perimeter. + /// \return Sum of the triangle's line segments. + public: T Perimeter() const + { + return this->Side(0).Length() + this->Side(1).Length() + + this->Side(2).Length(); + } + + /// \brief Get the area of this triangle. + /// \return Triangle's area. + public: double Area() const + { + double s = this->Perimeter() / 2.0; + T a = this->Side(0).Length(); + T b = this->Side(1).Length(); + T c = this->Side(2).Length(); + + // Heron's formula + // http://en.wikipedia.org/wiki/Heron%27s_formula + return sqrt(s * (s-a) * (s-b) * (s-c)); + } + + /// \brief Get one of points that define the triangle. + /// \param[in] _index: 0, 1, or 2. _index is clamped to the range + /// [0,2]. + /// \return The triangle point at _index. + public: Vector3 operator[](const size_t _index) const + { + return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; + } + + /// The points of the triangle + private: Vector3 pts[3]; + }; + + /// \brief Integer specialization of the Triangle class. + typedef Triangle3 Triangle3i; + + /// \brief Double specialization of the Triangle class. + typedef Triangle3 Triangle3d; + + /// \brief Float specialization of the Triangle class. + typedef Triangle3 Triangle3f; + } + } +} +#endif diff --git a/include/gz/math/Vector2.hh b/include/gz/math/Vector2.hh new file mode 100644 index 000000000..a30780244 --- /dev/null +++ b/include/gz/math/Vector2.hh @@ -0,0 +1,598 @@ +/* + * Copyright (C) 2012 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_VECTOR2_HH_ +#define GZ_MATH_VECTOR2_HH_ + +#include +#include +#include + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Vector2 Vector2.hh ignition/math/Vector2.hh + /// \brief Two dimensional (x, y) vector. + template + class Vector2 + { + /// \brief math::Vector2(0, 0) + public: static const Vector2 Zero; + + /// \brief math::Vector2(1, 1) + public: static const Vector2 One; + + /// \brief math::Vector2(NaN, NaN, NaN) + public: static const Vector2 NaN; + + /// \brief Default Constructor + public: Vector2() + { + this->data[0] = 0; + this->data[1] = 0; + } + + /// \brief Constructor + /// \param[in] _x value along x + /// \param[in] _y value along y + public: Vector2(const T &_x, const T &_y) + { + this->data[0] = _x; + this->data[1] = _y; + } + + /// \brief Copy constructor + /// \param[in] _v the value + public: Vector2(const Vector2 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + } + + /// \brief Destructor + public: virtual ~Vector2() {} + + /// \brief Return the sum of the values + /// \return the sum + public: T Sum() const + { + return this->data[0] + this->data[1]; + } + + /// \brief Calc distance to the given point + /// \param[in] _pt The point to measure to + /// \return the distance + public: double Distance(const Vector2 &_pt) const + { + return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + + (this->data[1]-_pt[1])*(this->data[1]-_pt[1])); + } + + /// \brief Returns the length (magnitude) of the vector + /// \return The length + public: T Length() const + { + return static_cast(sqrt(this->SquaredLength())); + } + + /// \brief Returns the square of the length (magnitude) of the vector + /// \return The squared length + public: T SquaredLength() const + { + return + this->data[0] * this->data[0] + + this->data[1] * this->data[1]; + } + + /// \brief Normalize the vector length + public: void Normalize() + { + T d = this->Length(); + + if (!equal(d, static_cast(0.0))) + { + this->data[0] /= d; + this->data[1] /= d; + } + } + + /// \brief Returns a normalized vector + /// \return unit length vector + public: Vector2 Normalized() const + { + Vector2 result = *this; + result.Normalize(); + return result; + } + + /// \brief Round to near whole number, return the result. + /// \return the result + public: Vector2 Round() + { + this->data[0] = static_cast(std::nearbyint(this->data[0])); + this->data[1] = static_cast(std::nearbyint(this->data[1])); + return *this; + } + + /// \brief Get a rounded version of this vector + /// \return a rounded vector + public: Vector2 Rounded() const + { + Vector2 result = *this; + result.Round(); + return result; + } + + /// \brief Set the contents of the vector + /// \param[in] _x value along x + /// \param[in] _y value along y + public: void Set(T _x, T _y) + { + this->data[0] = _x; + this->data[1] = _y; + } + + /// \brief Get the dot product of this vector and _v + /// \param[in] _v the vector + /// \return The dot product + public: T Dot(const Vector2 &_v) const + { + return (this->data[0] * _v[0]) + (this->data[1] * _v[1]); + } + + /// \brief Get the absolute value of the vector + /// \return a vector with positive elements + public: Vector2 Abs() const + { + return Vector2(std::abs(this->data[0]), + std::abs(this->data[1])); + } + + /// \brief Return the absolute dot product of this vector and + /// another vector. This is similar to the Dot function, except the + /// absolute value of each component of the vector is used. + /// + /// result = abs(x1 * x2) + abs(y1 * y2) + /// + /// \param[in] _v The vector + /// \return The absolute dot product + public: T AbsDot(const Vector2 &_v) const + { + return std::abs(this->data[0] * _v[0]) + + std::abs(this->data[1] * _v[1]); + } + + /// \brief Corrects any nan values + public: inline void Correct() + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + if (!std::isfinite(static_cast(this->data[0]))) + this->data[0] = 0; + if (!std::isfinite(static_cast(this->data[1]))) + this->data[1] = 0; + } + + /// \brief Set this vector's components to the maximum of itself and the + /// passed in vector + /// \param[in] _v the maximum clamping vector + public: void Max(const Vector2 &_v) + { + this->data[0] = std::max(_v[0], this->data[0]); + this->data[1] = std::max(_v[1], this->data[1]); + } + + /// \brief Set this vector's components to the minimum of itself and the + /// passed in vector + /// \param[in] _v the minimum clamping vector + public: void Min(const Vector2 &_v) + { + this->data[0] = std::min(_v[0], this->data[0]); + this->data[1] = std::min(_v[1], this->data[1]); + } + + /// \brief Get the maximum value in the vector + /// \return the maximum element + public: T Max() const + { + return std::max(this->data[0], this->data[1]); + } + + /// \brief Get the minimum value in the vector + /// \return the minimum element + public: T Min() const + { + return std::min(this->data[0], this->data[1]); + } + + /// \brief Assignment operator + /// \param[in] _v a value for x and y element + /// \return this + public: Vector2 &operator=(const Vector2 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + + return *this; + } + + /// \brief Assignment operator + /// \param[in] _v the value for x and y element + /// \return this + public: const Vector2 &operator=(T _v) + { + this->data[0] = _v; + this->data[1] = _v; + + return *this; + } + + /// \brief Addition operator + /// \param[in] _v vector to add + /// \return sum vector + public: Vector2 operator+(const Vector2 &_v) const + { + return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]); + } + + /// \brief Addition assignment operator + /// \param[in] _v the vector to add + // \return this + public: const Vector2 &operator+=(const Vector2 &_v) + { + this->data[0] += _v[0]; + this->data[1] += _v[1]; + + return *this; + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \return sum vector + public: inline Vector2 operator+(const T _s) const + { + return Vector2(this->data[0] + _s, + this->data[1] + _s); + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \param[in] _v input vector + /// \return sum vector + public: friend inline Vector2 operator+(const T _s, + const Vector2 &_v) + { + return _v + _s; + } + + /// \brief Addition assignment operator + /// \param[in] _s scalar addend + /// \return this + public: const Vector2 &operator+=(const T _s) + { + this->data[0] += _s; + this->data[1] += _s; + + return *this; + } + + /// \brief Negation operator + /// \return negative of this vector + public: inline Vector2 operator-() const + { + return Vector2(-this->data[0], -this->data[1]); + } + + /// \brief Subtraction operator + /// \param[in] _v the vector to substract + /// \return the subtracted vector + public: Vector2 operator-(const Vector2 &_v) const + { + return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]); + } + + /// \brief Subtraction assignment operator + /// \param[in] _v the vector to substract + /// \return this + public: const Vector2 &operator-=(const Vector2 &_v) + { + this->data[0] -= _v[0]; + this->data[1] -= _v[1]; + + return *this; + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar subtrahend + /// \return difference vector + public: inline Vector2 operator-(const T _s) const + { + return Vector2(this->data[0] - _s, + this->data[1] - _s); + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar minuend + /// \param[in] _v vector subtrahend + /// \return difference vector + public: friend inline Vector2 operator-(const T _s, + const Vector2 &_v) + { + return {_s - _v.X(), _s - _v.Y()}; + } + + /// \brief Subtraction assignment operator + /// \param[in] _s scalar subtrahend + /// \return this + public: const Vector2 &operator-=(T _s) + { + this->data[0] -= _s; + this->data[1] -= _s; + + return *this; + } + + /// \brief Division operator + /// \remarks this is an element wise division + /// \param[in] _v a vector + /// \result a result + public: const Vector2 operator/(const Vector2 &_v) const + { + return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]); + } + + /// \brief Division operator + /// \remarks this is an element wise division + /// \param[in] _v a vector + /// \return this + public: const Vector2 &operator/=(const Vector2 &_v) + { + this->data[0] /= _v[0]; + this->data[1] /= _v[1]; + + return *this; + } + + /// \brief Division operator + /// \param[in] _v the value + /// \return a vector + public: const Vector2 operator/(T _v) const + { + return Vector2(this->data[0] / _v, this->data[1] / _v); + } + + /// \brief Division operator + /// \param[in] _v the divisor + /// \return a vector + public: const Vector2 &operator/=(T _v) + { + this->data[0] /= _v; + this->data[1] /= _v; + + return *this; + } + + /// \brief Multiplication operators + /// \param[in] _v the vector + /// \return the result + public: const Vector2 operator*(const Vector2 &_v) const + { + return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]); + } + + /// \brief Multiplication assignment operator + /// \remarks this is an element wise multiplication + /// \param[in] _v the vector + /// \return this + public: const Vector2 &operator*=(const Vector2 &_v) + { + this->data[0] *= _v[0]; + this->data[1] *= _v[1]; + + return *this; + } + + /// \brief Multiplication operators + /// \param[in] _v the scaling factor + /// \return a scaled vector + public: const Vector2 operator*(T _v) const + { + return Vector2(this->data[0] * _v, this->data[1] * _v); + } + + /// \brief Scalar left multiplication operators + /// \param[in] _s the scaling factor + /// \param[in] _v the vector to scale + /// \return a scaled vector + public: friend inline const Vector2 operator*(const T _s, + const Vector2 &_v) + { + return Vector2(_v * _s); + } + + /// \brief Multiplication assignment operator + /// \param[in] _v the scaling factor + /// \return a scaled vector + public: const Vector2 &operator*=(T _v) + { + this->data[0] *= _v; + this->data[1] *= _v; + + return *this; + } + + /// \brief Equality test with tolerance. + /// \param[in] _v the vector to compare to + /// \param[in] _tol equality tolerance. + /// \return true if the elements of the vectors are equal within + /// the tolerence specified by _tol. + public: bool Equal(const Vector2 &_v, const T &_tol) const + { + return equal(this->data[0], _v[0], _tol) + && equal(this->data[1], _v[1], _tol); + } + + /// \brief Equal to operator + /// \param[in] _v the vector to compare to + /// \return true if the elements of the 2 vectors are equal within + /// a tolerence (1e-6) + public: bool operator==(const Vector2 &_v) const + { + return this->Equal(_v, static_cast(1e-6)); + } + + /// \brief Not equal to operator + /// \return true if elements are of diffent values (tolerence 1e-6) + public: bool operator!=(const Vector2 &_v) const + { + return !(*this == _v); + } + + /// \brief See if a point is finite (e.g., not nan) + /// \return true if finite, false otherwise + public: bool IsFinite() const + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + return std::isfinite(static_cast(this->data[0])) && + std::isfinite(static_cast(this->data[1])); + } + + /// \brief Array subscript operator + /// \param[in] _index The index, where 0 == x and 1 == y. + /// The index is clamped to the range [0,1]. + public: T &operator[](const std::size_t _index) + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; + } + + /// \brief Const-qualified array subscript operator + /// \param[in] _index The index, where 0 == x and 1 == y. + /// The index is clamped to the range [0,1]. + public: T operator[](const std::size_t _index) const + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; + } + + /// \brief Return the x value. + /// \return Value of the X component. + public: inline T X() const + { + return this->data[0]; + } + + /// \brief Return the y value. + /// \return Value of the Y component. + public: inline T Y() const + { + return this->data[1]; + } + + /// \brief Return a mutable x value. + /// \return Value of the X component. + public: inline T &X() + { + return this->data[0]; + } + + /// \brief Return a mutable y value. + /// \return Value of the Y component. + public: inline T &Y() + { + return this->data[1]; + } + + /// \brief Set the x value. + /// \param[in] _v Value for the x component. + public: inline void X(const T &_v) + { + this->data[0] = _v; + } + + /// \brief Set the y value. + /// \param[in] _v Value for the y component. + public: inline void Y(const T &_v) + { + this->data[1] = _v; + } + + /// \brief Stream extraction operator + /// \param[in] _out output stream + /// \param[in] _pt Vector2 to output + /// \return The stream + public: friend std::ostream + &operator<<(std::ostream &_out, const Vector2 &_pt) + { + _out << _pt[0] << " " << _pt[1]; + return _out; + } + + /// \brief Less than operator. + /// \param[in] _pt Vector to compare. + /// \return True if this vector's first or second value is less than + /// the given vector's first or second value. + public: bool operator<(const Vector2 &_pt) const + { + return this->data[0] < _pt[0] || this->data[1] < _pt[1]; + } + + /// \brief Stream extraction operator + /// \param[in] _in input stream + /// \param[in] _pt Vector2 to read values into + /// \return The stream + public: friend std::istream + &operator>>(std::istream &_in, Vector2 &_pt) + { + T x, y; + // Skip white spaces + _in.setf(std::ios_base::skipws); + _in >> x >> y; + if (!_in.fail()) + { + _pt.Set(x, y); + } + return _in; + } + + /// \brief The x and y values. + private: T data[2]; + }; + + template + const Vector2 Vector2::Zero(0, 0); + + template + const Vector2 Vector2::One(1, 1); + + template + const Vector2 Vector2::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + + typedef Vector2 Vector2i; + typedef Vector2 Vector2d; + typedef Vector2 Vector2f; + } + } +} +#endif diff --git a/include/gz/math/Vector3.hh b/include/gz/math/Vector3.hh new file mode 100644 index 000000000..3c286c295 --- /dev/null +++ b/include/gz/math/Vector3.hh @@ -0,0 +1,775 @@ +/* + * Copyright (C) 2014 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_VECTOR3_HH_ +#define GZ_MATH_VECTOR3_HH_ + +#include +#include +#include +#include +#include + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Vector3 Vector3.hh ignition/math/Vector3.hh + /// \brief The Vector3 class represents the generic vector containing 3 + /// elements. Since it's commonly used to keep coordinate system + /// related information, its elements are labeled by x, y, z. + template + class Vector3 + { + /// \brief math::Vector3(0, 0, 0) + public: static const Vector3 Zero; + + /// \brief math::Vector3(1, 1, 1) + public: static const Vector3 One; + + /// \brief math::Vector3(1, 0, 0) + public: static const Vector3 UnitX; + + /// \brief math::Vector3(0, 1, 0) + public: static const Vector3 UnitY; + + /// \brief math::Vector3(0, 0, 1) + public: static const Vector3 UnitZ; + + /// \brief math::Vector3(NaN, NaN, NaN) + public: static const Vector3 NaN; + + /// \brief Constructor + public: Vector3() + { + this->data[0] = 0; + this->data[1] = 0; + this->data[2] = 0; + } + + /// \brief Constructor + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value along z + public: Vector3(const T &_x, const T &_y, const T &_z) + { + this->data[0] = _x; + this->data[1] = _y; + this->data[2] = _z; + } + + /// \brief Copy constructor + /// \param[in] _v a vector + public: Vector3(const Vector3 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + this->data[2] = _v[2]; + } + + /// \brief Destructor + public: virtual ~Vector3() {} + + /// \brief Return the sum of the values + /// \return the sum + public: T Sum() const + { + return this->data[0] + this->data[1] + this->data[2]; + } + + /// \brief Calc distance to the given point + /// \param[in] _pt the point + /// \return the distance + public: T Distance(const Vector3 &_pt) const + { + return static_cast(sqrt( + (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + + (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) + + (this->data[2]-_pt[2])*(this->data[2]-_pt[2]))); + } + + /// \brief Calc distance to the given point + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value along z + /// \return the distance + public: T Distance(T _x, T _y, T _z) const + { + return this->Distance(Vector3(_x, _y, _z)); + } + + /// \brief Returns the length (magnitude) of the vector + /// \return the length + public: T Length() const + { + return static_cast(sqrt(this->SquaredLength())); + } + + /// \brief Return the square of the length (magnitude) of the vector + /// \return the squared length + public: T SquaredLength() const + { + return + this->data[0] * this->data[0] + + this->data[1] * this->data[1] + + this->data[2] * this->data[2]; + } + + /// \brief Normalize the vector length + /// \return unit length vector + public: Vector3 Normalize() + { + T d = this->Length(); + + if (!equal(d, static_cast(0.0))) + { + this->data[0] /= d; + this->data[1] /= d; + this->data[2] /= d; + } + + return *this; + } + + /// \brief Return a normalized vector + /// \return unit length vector + public: Vector3 Normalized() const + { + Vector3 result = *this; + result.Normalize(); + return result; + } + + /// \brief Round to near whole number, return the result. + /// \return the result + public: Vector3 Round() + { + this->data[0] = static_cast(std::nearbyint(this->data[0])); + this->data[1] = static_cast(std::nearbyint(this->data[1])); + this->data[2] = static_cast(std::nearbyint(this->data[2])); + return *this; + } + + /// \brief Get a rounded version of this vector + /// \return a rounded vector + public: Vector3 Rounded() const + { + Vector3 result = *this; + result.Round(); + return result; + } + + /// \brief Set the contents of the vector + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value aling z + public: inline void Set(T _x = 0, T _y = 0, T _z = 0) + { + this->data[0] = _x; + this->data[1] = _y; + this->data[2] = _z; + } + + /// \brief Return the cross product of this vector with another vector. + /// \param[in] _v a vector + /// \return the cross product + public: Vector3 Cross(const Vector3 &_v) const + { + return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1], + this->data[2] * _v[0] - this->data[0] * _v[2], + this->data[0] * _v[1] - this->data[1] * _v[0]); + } + + /// \brief Return the dot product of this vector and another vector + /// \param[in] _v the vector + /// \return the dot product + public: T Dot(const Vector3 &_v) const + { + return this->data[0] * _v[0] + + this->data[1] * _v[1] + + this->data[2] * _v[2]; + } + + /// \brief Return the absolute dot product of this vector and + /// another vector. This is similar to the Dot function, except the + /// absolute value of each component of the vector is used. + /// + /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2) + /// + /// \param[in] _v the vector + /// \return The absolute dot product + public: T AbsDot(const Vector3 &_v) const + { + return std::abs(this->data[0] * _v[0]) + + std::abs(this->data[1] * _v[1]) + + std::abs(this->data[2] * _v[2]); + } + + /// \brief Get the absolute value of the vector + /// \return a vector with positive elements + public: Vector3 Abs() const + { + return Vector3(std::abs(this->data[0]), + std::abs(this->data[1]), + std::abs(this->data[2])); + } + + /// \brief Return a vector that is perpendicular to this one. + /// \return an orthogonal vector + public: Vector3 Perpendicular() const + { + static const T sqrZero = static_cast(1e-06 * 1e-06); + + Vector3 perp = this->Cross(Vector3(1, 0, 0)); + + // Check the length of the vector + if (perp.SquaredLength() < sqrZero) + { + perp = this->Cross(Vector3(0, 1, 0)); + } + + return perp; + } + + /// \brief Get a normal vector to a triangle + /// \param[in] _v1 first vertex of the triangle + /// \param[in] _v2 second vertex + /// \param[in] _v3 third vertex + /// \return the normal + public: static Vector3 Normal(const Vector3 &_v1, + const Vector3 &_v2, const Vector3 &_v3) + { + Vector3 a = _v2 - _v1; + Vector3 b = _v3 - _v1; + Vector3 n = a.Cross(b); + return n.Normalize(); + } + + /// \brief Get distance to an infinite line defined by 2 points. + /// \param[in] _pt1 first point on the line + /// \param[in] _pt2 second point on the line + /// \return the minimum distance from this point to the line + public: T DistToLine(const Vector3 &_pt1, const Vector3 &_pt2) + { + T d = ((*this) - _pt1).Cross((*this) - _pt2).Length(); + d = d / (_pt2 - _pt1).Length(); + return d; + } + + /// \brief Set this vector's components to the maximum of itself and the + /// passed in vector + /// \param[in] _v the maximum clamping vector + public: void Max(const Vector3 &_v) + { + if (_v[0] > this->data[0]) + this->data[0] = _v[0]; + if (_v[1] > this->data[1]) + this->data[1] = _v[1]; + if (_v[2] > this->data[2]) + this->data[2] = _v[2]; + } + + /// \brief Set this vector's components to the minimum of itself and the + /// passed in vector + /// \param[in] _v the minimum clamping vector + public: void Min(const Vector3 &_v) + { + if (_v[0] < this->data[0]) + this->data[0] = _v[0]; + if (_v[1] < this->data[1]) + this->data[1] = _v[1]; + if (_v[2] < this->data[2]) + this->data[2] = _v[2]; + } + + /// \brief Get the maximum value in the vector + /// \return the maximum element + public: T Max() const + { + return std::max(std::max(this->data[0], this->data[1]), this->data[2]); + } + + /// \brief Get the minimum value in the vector + /// \return the minimum element + public: T Min() const + { + return std::min(std::min(this->data[0], this->data[1]), this->data[2]); + } + + /// \brief Assignment operator + /// \param[in] _v a new value + /// \return this + public: Vector3 &operator=(const Vector3 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + this->data[2] = _v[2]; + + return *this; + } + + /// \brief Assignment operator + /// \param[in] _v assigned to all elements + /// \return this + public: Vector3 &operator=(T _v) + { + this->data[0] = _v; + this->data[1] = _v; + this->data[2] = _v; + + return *this; + } + + /// \brief Addition operator + /// \param[in] _v vector to add + /// \return the sum vector + public: Vector3 operator+(const Vector3 &_v) const + { + return Vector3(this->data[0] + _v[0], + this->data[1] + _v[1], + this->data[2] + _v[2]); + } + + /// \brief Addition assignment operator + /// \param[in] _v vector to add + /// \return the sum vector + public: const Vector3 &operator+=(const Vector3 &_v) + { + this->data[0] += _v[0]; + this->data[1] += _v[1]; + this->data[2] += _v[2]; + + return *this; + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \return sum vector + public: inline Vector3 operator+(const T _s) const + { + return Vector3(this->data[0] + _s, + this->data[1] + _s, + this->data[2] + _s); + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \param[in] _v input vector + /// \return sum vector + public: friend inline Vector3 operator+(const T _s, + const Vector3 &_v) + { + return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s}; + } + + /// \brief Addition assignment operator + /// \param[in] _s scalar addend + /// \return this + public: const Vector3 &operator+=(const T _s) + { + this->data[0] += _s; + this->data[1] += _s; + this->data[2] += _s; + + return *this; + } + + /// \brief Negation operator + /// \return negative of this vector + public: inline Vector3 operator-() const + { + return Vector3(-this->data[0], -this->data[1], -this->data[2]); + } + + /// \brief Subtraction operators + /// \param[in] _pt a vector to substract + /// \return a vector after the substraction + public: inline Vector3 operator-(const Vector3 &_pt) const + { + return Vector3(this->data[0] - _pt[0], + this->data[1] - _pt[1], + this->data[2] - _pt[2]); + } + + /// \brief Subtraction assignment operators + /// \param[in] _pt subtrahend + /// \return a vector after the substraction + public: const Vector3 &operator-=(const Vector3 &_pt) + { + this->data[0] -= _pt[0]; + this->data[1] -= _pt[1]; + this->data[2] -= _pt[2]; + + return *this; + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar subtrahend + /// \return difference vector + public: inline Vector3 operator-(const T _s) const + { + return Vector3(this->data[0] - _s, + this->data[1] - _s, + this->data[2] - _s); + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar minuend + /// \param[in] _v vector subtrahend + /// \return difference vector + public: friend inline Vector3 operator-(const T _s, + const Vector3 &_v) + { + return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()}; + } + + /// \brief Subtraction assignment operator + /// \param[in] _s scalar subtrahend + /// \return this + public: const Vector3 &operator-=(const T _s) + { + this->data[0] -= _s; + this->data[1] -= _s; + this->data[2] -= _s; + + return *this; + } + + /// \brief Division operator + /// \remarks this is an element wise division + /// \param[in] _pt the vector divisor + /// \return a vector + public: const Vector3 operator/(const Vector3 &_pt) const + { + return Vector3(this->data[0] / _pt[0], + this->data[1] / _pt[1], + this->data[2] / _pt[2]); + } + + /// \brief Division assignment operator + /// \remarks this is an element wise division + /// \param[in] _pt the vector divisor + /// \return a vector + public: const Vector3 &operator/=(const Vector3 &_pt) + { + this->data[0] /= _pt[0]; + this->data[1] /= _pt[1]; + this->data[2] /= _pt[2]; + + return *this; + } + + /// \brief Division operator + /// \remarks this is an element wise division + /// \param[in] _v the divisor + /// \return a vector + public: const Vector3 operator/(T _v) const + { + return Vector3(this->data[0] / _v, + this->data[1] / _v, + this->data[2] / _v); + } + + /// \brief Division assignment operator + /// \remarks this is an element wise division + /// \param[in] _v the divisor + /// \return this + public: const Vector3 &operator/=(T _v) + { + this->data[0] /= _v; + this->data[1] /= _v; + this->data[2] /= _v; + + return *this; + } + + /// \brief Multiplication operator + /// \remarks this is an element wise multiplication, not a cross product + /// \param[in] _p multiplier operator + /// \return a vector + public: Vector3 operator*(const Vector3 &_p) const + { + return Vector3(this->data[0] * _p[0], + this->data[1] * _p[1], + this->data[2] * _p[2]); + } + + /// \brief Multiplication assignment operators + /// \remarks this is an element wise multiplication, not a cross product + /// \param[in] _v a vector + /// \return this + public: const Vector3 &operator*=(const Vector3 &_v) + { + this->data[0] *= _v[0]; + this->data[1] *= _v[1]; + this->data[2] *= _v[2]; + + return *this; + } + + /// \brief Multiplication operators + /// \param[in] _s the scaling factor + /// \return a scaled vector + public: inline Vector3 operator*(T _s) const + { + return Vector3(this->data[0] * _s, + this->data[1] * _s, + this->data[2] * _s); + } + + /// \brief Multiplication operators + /// \param[in] _s the scaling factor + /// \param[in] _v input vector + /// \return a scaled vector + public: friend inline Vector3 operator*(T _s, const Vector3 &_v) + { + return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s}; + } + + /// \brief Multiplication operator + /// \param[in] _v scaling factor + /// \return this + public: const Vector3 &operator*=(T _v) + { + this->data[0] *= _v; + this->data[1] *= _v; + this->data[2] *= _v; + + return *this; + } + + /// \brief Equality test with tolerance. + /// \param[in] _v the vector to compare to + /// \param[in] _tol equality tolerance. + /// \return true if the elements of the vectors are equal within + /// the tolerence specified by _tol. + public: bool Equal(const Vector3 &_v, const T &_tol) const + { + return equal(this->data[0], _v[0], _tol) + && equal(this->data[1], _v[1], _tol) + && equal(this->data[2], _v[2], _tol); + } + + /// \brief Equal to operator + /// \param[in] _v The vector to compare against + /// \return true if each component is equal within a + /// default tolerence (1e-3), false otherwise + public: bool operator==(const Vector3 &_v) const + { + return this->Equal(_v, static_cast(1e-3)); + } + + /// \brief Not equal to operator + /// \param[in] _v The vector to compare against + /// \return false if each component is equal within a + /// default tolerence (1e-3), true otherwise + public: bool operator!=(const Vector3 &_v) const + { + return !(*this == _v); + } + + /// \brief See if a point is finite (e.g., not nan) + /// \return true if is finite or false otherwise + public: bool IsFinite() const + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + return std::isfinite(static_cast(this->data[0])) && + std::isfinite(static_cast(this->data[1])) && + std::isfinite(static_cast(this->data[2])); + } + + /// \brief Corrects any nan values + public: inline void Correct() + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + if (!std::isfinite(static_cast(this->data[0]))) + this->data[0] = 0; + if (!std::isfinite(static_cast(this->data[1]))) + this->data[1] = 0; + if (!std::isfinite(static_cast(this->data[2]))) + this->data[2] = 0; + } + + /// \brief Array subscript operator + /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z. + /// The index is clamped to the range [0,2]. + /// \return The value. + public: T &operator[](const std::size_t _index) + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; + } + + /// \brief Const-qualified array subscript operator + /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z. + /// The index is clamped to the range [0,2]. + /// \return The value. + public: T operator[](const std::size_t _index) const + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; + } + + /// \brief Round all values to _precision decimal places + /// \param[in] _precision the decimal places + public: void Round(int _precision) + { + this->data[0] = precision(this->data[0], _precision); + this->data[1] = precision(this->data[1], _precision); + this->data[2] = precision(this->data[2], _precision); + } + + /// \brief Equality test + /// \remarks This is equivalent to the == operator + /// \param[in] _v the other vector + /// \return true if the 2 vectors have the same values, false otherwise + public: bool Equal(const Vector3 &_v) const + { + return equal(this->data[0], _v[0]) && + equal(this->data[1], _v[1]) && + equal(this->data[2], _v[2]); + } + + /// \brief Get the x value. + /// \return The x component of the vector + public: inline T X() const + { + return this->data[0]; + } + + /// \brief Get the y value. + /// \return The y component of the vector + public: inline T Y() const + { + return this->data[1]; + } + + /// \brief Get the z value. + /// \return The z component of the vector + public: inline T Z() const + { + return this->data[2]; + } + + /// \brief Get a mutable reference to the x value. + /// \return The x component of the vector + public: inline T &X() + { + return this->data[0]; + } + + /// \brief Get a mutable reference to the y value. + /// \return The y component of the vector + public: inline T &Y() + { + return this->data[1]; + } + + /// \brief Get a mutable reference to the z value. + /// \return The z component of the vector + public: inline T &Z() + { + return this->data[2]; + } + + /// \brief Set the x value. + /// \param[in] _v Value for the x component. + public: inline void X(const T &_v) + { + this->data[0] = _v; + } + + /// \brief Set the y value. + /// \param[in] _v Value for the y component. + public: inline void Y(const T &_v) + { + this->data[1] = _v; + } + + /// \brief Set the z value. + /// \param[in] _v Value for the z component. + public: inline void Z(const T &_v) + { + this->data[2] = _v; + } + + /// \brief Less than operator. + /// \param[in] _pt Vector to compare. + /// \return True if this vector's X(), Y(), or Z() value is less + /// than the given vector's corresponding values. + public: bool operator<(const Vector3 &_pt) const + { + return this->data[0] < _pt[0] || this->data[1] < _pt[1] || + this->data[2] < _pt[2]; + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _pt Vector3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Vector3 &_pt) + { + _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " " + << precision(_pt[2], 6); + return _out; + } + + /// \brief Stream extraction operator + /// \param _in input stream + /// \param _pt vector3 to read values into + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, gz::math::Vector3 &_pt) + { + // Skip white spaces + _in.setf(std::ios_base::skipws); + T x, y, z; + _in >> x >> y >> z; + if (!_in.fail()) + { + _pt.Set(x, y, z); + } + return _in; + } + + /// \brief The x, y, and z values + private: T data[3]; + }; + + template const Vector3 Vector3::Zero(0, 0, 0); + template const Vector3 Vector3::One(1, 1, 1); + template const Vector3 Vector3::UnitX(1, 0, 0); + template const Vector3 Vector3::UnitY(0, 1, 0); + template const Vector3 Vector3::UnitZ(0, 0, 1); + template const Vector3 Vector3::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + + typedef Vector3 Vector3i; + typedef Vector3 Vector3d; + typedef Vector3 Vector3f; + } + } +} +#endif diff --git a/include/gz/math/Vector3Stats.hh b/include/gz/math/Vector3Stats.hh new file mode 100644 index 000000000..d34f55f1e --- /dev/null +++ b/include/gz/math/Vector3Stats.hh @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2015 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_VECTOR3STATS_HH_ +#define GZ_MATH_VECTOR3STATS_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \brief Forward declare private data class. + class Vector3StatsPrivate; + + /// \class Vector3Stats Vector3Stats.hh ignition/math/Vector3Stats.hh + /// \brief Collection of statistics for a Vector3 signal. + class IGNITION_MATH_VISIBLE Vector3Stats + { + /// \brief Constructor + public: Vector3Stats(); + + /// \brief Destructor + public: ~Vector3Stats(); + + /// \brief Add a new sample to the statistical measures. + /// \param[in] _data New signal data point. + public: void InsertData(const Vector3d &_data); + + /// \brief Add a new type of statistic. + /// \param[in] _name Short name of new statistic. + /// Valid values include: + /// "maxAbs" + /// "mean" + /// "rms" + /// \return True if statistic was successfully added, + /// false if name was not recognized or had already + /// been inserted. + public: bool InsertStatistic(const std::string &_name); + + /// \brief Add multiple statistics. + /// \param[in] _names Comma-separated list of new statistics. + /// For example, all statistics could be added with: + /// "maxAbs,mean,rms" + /// \return True if all statistics were successfully added, + /// false if any names were not recognized or had already + /// been inserted. + public: bool InsertStatistics(const std::string &_names); + + /// \brief Forget all previous data. + public: void Reset(); + + /// \brief Get statistics for x component of signal. + /// \return Statistics for x component of signal. + public: const SignalStats &X() const; + + /// \brief Get statistics for y component of signal. + /// \return Statistics for y component of signal. + public: const SignalStats &Y() const; + + /// \brief Get statistics for z component of signal. + /// \return Statistics for z component of signal. + public: const SignalStats &Z() const; + + /// \brief Get statistics for magnitude component of signal. + /// \return Statistics for magnitude component of signal. + public: const SignalStats &Mag() const; + + /// \brief Get mutable reference to statistics for x component of signal. + /// \return Statistics for x component of signal. + public: SignalStats &X(); + + /// \brief Get mutable reference to statistics for y component of signal. + /// \return Statistics for y component of signal. + public: SignalStats &Y(); + + /// \brief Get mutable reference to statistics for z component of signal. + /// \return Statistics for z component of signal. + public: SignalStats &Z(); + + /// \brief Get mutable reference to statistics for magnitude of signal. + /// \return Statistics for magnitude of signal. + public: SignalStats &Mag(); + + /// \brief Pointer to private data. + protected: Vector3StatsPrivate *dataPtr; + }; + } + } +} +#endif + diff --git a/include/gz/math/Vector4.hh b/include/gz/math/Vector4.hh new file mode 100644 index 000000000..091b9b1c7 --- /dev/null +++ b/include/gz/math/Vector4.hh @@ -0,0 +1,755 @@ +/* + * Copyright (C) 2012 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_VECTOR4_HH_ +#define GZ_MATH_VECTOR4_HH_ + +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Vector4 Vector4.hh ignition/math/Vector4.hh + /// \brief T Generic x, y, z, w vector + template + class Vector4 + { + /// \brief math::Vector4(0, 0, 0, 0) + public: static const Vector4 Zero; + + /// \brief math::Vector4(1, 1, 1, 1) + public: static const Vector4 One; + + /// \brief math::Vector4(NaN, NaN, NaN, NaN) + public: static const Vector4 NaN; + + /// \brief Constructor + public: Vector4() + { + this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0; + } + + /// \brief Constructor with component values + /// \param[in] _x value along x axis + /// \param[in] _y value along y axis + /// \param[in] _z value along z axis + /// \param[in] _w value along w axis + public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w) + { + this->data[0] = _x; + this->data[1] = _y; + this->data[2] = _z; + this->data[3] = _w; + } + + /// \brief Copy constructor + /// \param[in] _v vector + public: Vector4(const Vector4 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + this->data[2] = _v[2]; + this->data[3] = _v[3]; + } + + /// \brief Destructor + public: virtual ~Vector4() {} + + /// \brief Calc distance to the given point + /// \param[in] _pt the point + /// \return the distance + public: T Distance(const Vector4 &_pt) const + { + return static_cast(sqrt( + (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + + (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) + + (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) + + (this->data[3]-_pt[3])*(this->data[3]-_pt[3]))); + } + + /// \brief Calc distance to the given point + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value along z + /// \param[in] _w value along w + /// \return the distance + public: T Distance(T _x, T _y, T _z, T _w) const + { + return this->Distance(Vector4(_x, _y, _z, _w)); + } + + /// \brief Returns the length (magnitude) of the vector + /// \return The length + public: T Length() const + { + return static_cast(sqrt(this->SquaredLength())); + } + + /// \brief Return the square of the length (magnitude) of the vector + /// \return the length + public: T SquaredLength() const + { + return + this->data[0] * this->data[0] + + this->data[1] * this->data[1] + + this->data[2] * this->data[2] + + this->data[3] * this->data[3]; + } + + /// \brief Round to near whole number. + public: void Round() + { + this->data[0] = static_cast(std::nearbyint(this->data[0])); + this->data[1] = static_cast(std::nearbyint(this->data[1])); + this->data[2] = static_cast(std::nearbyint(this->data[2])); + this->data[3] = static_cast(std::nearbyint(this->data[3])); + } + + /// \brief Get a rounded version of this vector + /// \return a rounded vector + public: Vector4 Rounded() const + { + Vector4 result = *this; + result.Round(); + return result; + } + + /// \brief Corrects any nan values + public: inline void Correct() + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + if (!std::isfinite(static_cast(this->data[0]))) + this->data[0] = 0; + if (!std::isfinite(static_cast(this->data[1]))) + this->data[1] = 0; + if (!std::isfinite(static_cast(this->data[2]))) + this->data[2] = 0; + if (!std::isfinite(static_cast(this->data[3]))) + this->data[3] = 0; + } + + /// \brief Normalize the vector length + public: void Normalize() + { + T d = this->Length(); + + if (!equal(d, static_cast(0.0))) + { + this->data[0] /= d; + this->data[1] /= d; + this->data[2] /= d; + this->data[3] /= d; + } + } + + /// \brief Return a normalized vector + /// \return unit length vector + public: Vector4 Normalized() const + { + Vector4 result = *this; + result.Normalize(); + return result; + } + + /// \brief Return the dot product of this vector and another vector + /// \param[in] _v the vector + /// \return the dot product + public: T Dot(const Vector4 &_v) const + { + return this->data[0] * _v[0] + + this->data[1] * _v[1] + + this->data[2] * _v[2] + + this->data[3] * _v[3]; + } + + /// \brief Return the absolute dot product of this vector and + /// another vector. This is similar to the Dot function, except the + /// absolute value of each component of the vector is used. + /// + /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 * z2) + abs(w1 * w2) + /// + /// \param[in] _v the vector + /// \return The absolute dot product + public: T AbsDot(const Vector4 &_v) const + { + return std::abs(this->data[0] * _v[0]) + + std::abs(this->data[1] * _v[1]) + + std::abs(this->data[2] * _v[2]) + + std::abs(this->data[3] * _v[3]); + } + + /// \brief Get the absolute value of the vector + /// \return a vector with positive elements + public: Vector4 Abs() const + { + return Vector4(std::abs(this->data[0]), + std::abs(this->data[1]), + std::abs(this->data[2]), + std::abs(this->data[3])); + } + + /// \brief Set the contents of the vector + /// \param[in] _x value along x axis + /// \param[in] _y value along y axis + /// \param[in] _z value along z axis + /// \param[in] _w value along w axis + public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0) + { + this->data[0] = _x; + this->data[1] = _y; + this->data[2] = _z; + this->data[3] = _w; + } + + /// \brief Set this vector's components to the maximum of itself and the + /// passed in vector + /// \param[in] _v the maximum clamping vector + public: void Max(const Vector4 &_v) + { + this->data[0] = std::max(_v[0], this->data[0]); + this->data[1] = std::max(_v[1], this->data[1]); + this->data[2] = std::max(_v[2], this->data[2]); + this->data[3] = std::max(_v[3], this->data[3]); + } + + /// \brief Set this vector's components to the minimum of itself and the + /// passed in vector + /// \param[in] _v the minimum clamping vector + public: void Min(const Vector4 &_v) + { + this->data[0] = std::min(_v[0], this->data[0]); + this->data[1] = std::min(_v[1], this->data[1]); + this->data[2] = std::min(_v[2], this->data[2]); + this->data[3] = std::min(_v[3], this->data[3]); + } + + /// \brief Get the maximum value in the vector + /// \return the maximum element + public: T Max() const + { + return *std::max_element(this->data, this->data+4); + } + + /// \brief Get the minimum value in the vector + /// \return the minimum element + public: T Min() const + { + return *std::min_element(this->data, this->data+4); + } + + /// \brief Return the sum of the values + /// \return the sum + public: T Sum() const + { + return this->data[0] + this->data[1] + this->data[2] + this->data[3]; + } + + /// \brief Assignment operator + /// \param[in] _v the vector + /// \return a reference to this vector + public: Vector4 &operator=(const Vector4 &_v) + { + this->data[0] = _v[0]; + this->data[1] = _v[1]; + this->data[2] = _v[2]; + this->data[3] = _v[3]; + + return *this; + } + + /// \brief Assignment operator + /// \param[in] _value + public: Vector4 &operator=(T _value) + { + this->data[0] = _value; + this->data[1] = _value; + this->data[2] = _value; + this->data[3] = _value; + + return *this; + } + + /// \brief Addition operator + /// \param[in] _v the vector to add + /// \result a sum vector + public: Vector4 operator+(const Vector4 &_v) const + { + return Vector4(this->data[0] + _v[0], + this->data[1] + _v[1], + this->data[2] + _v[2], + this->data[3] + _v[3]); + } + + /// \brief Addition operator + /// \param[in] _v the vector to add + /// \return this vector + public: const Vector4 &operator+=(const Vector4 &_v) + { + this->data[0] += _v[0]; + this->data[1] += _v[1]; + this->data[2] += _v[2]; + this->data[3] += _v[3]; + + return *this; + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \return sum vector + public: inline Vector4 operator+(const T _s) const + { + return Vector4(this->data[0] + _s, + this->data[1] + _s, + this->data[2] + _s, + this->data[3] + _s); + } + + /// \brief Addition operators + /// \param[in] _s the scalar addend + /// \param[in] _v input vector + /// \return sum vector + public: friend inline Vector4 operator+(const T _s, + const Vector4 &_v) + { + return _v + _s; + } + + /// \brief Addition assignment operator + /// \param[in] _s scalar addend + /// \return this + public: const Vector4 &operator+=(const T _s) + { + this->data[0] += _s; + this->data[1] += _s; + this->data[2] += _s; + this->data[3] += _s; + + return *this; + } + + /// \brief Negation operator + /// \return negative of this vector + public: inline Vector4 operator-() const + { + return Vector4(-this->data[0], -this->data[1], + -this->data[2], -this->data[3]); + } + + /// \brief Subtraction operator + /// \param[in] _v the vector to substract + /// \return a vector + public: Vector4 operator-(const Vector4 &_v) const + { + return Vector4(this->data[0] - _v[0], + this->data[1] - _v[1], + this->data[2] - _v[2], + this->data[3] - _v[3]); + } + + /// \brief Subtraction assigment operators + /// \param[in] _v the vector to substract + /// \return this vector + public: const Vector4 &operator-=(const Vector4 &_v) + { + this->data[0] -= _v[0]; + this->data[1] -= _v[1]; + this->data[2] -= _v[2]; + this->data[3] -= _v[3]; + + return *this; + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar subtrahend + /// \return difference vector + public: inline Vector4 operator-(const T _s) const + { + return Vector4(this->data[0] - _s, + this->data[1] - _s, + this->data[2] - _s, + this->data[3] - _s); + } + + /// \brief Subtraction operators + /// \param[in] _s the scalar minuend + /// \param[in] _v vector subtrahend + /// \return difference vector + public: friend inline Vector4 operator-(const T _s, + const Vector4 &_v) + { + return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()}; + } + + /// \brief Subtraction assignment operator + /// \param[in] _s scalar subtrahend + /// \return this + public: const Vector4 &operator-=(const T _s) + { + this->data[0] -= _s; + this->data[1] -= _s; + this->data[2] -= _s; + this->data[3] -= _s; + + return *this; + } + + /// \brief Division assignment operator + /// \remarks Performs element wise division, + /// which has limited use. + /// \param[in] _v the vector to perform element wise division with + /// \return a result vector + public: const Vector4 operator/(const Vector4 &_v) const + { + return Vector4(this->data[0] / _v[0], + this->data[1] / _v[1], + this->data[2] / _v[2], + this->data[3] / _v[3]); + } + + /// \brief Division assignment operator + /// \remarks Performs element wise division, + /// which has limited use. + /// \param[in] _v the vector to perform element wise division with + /// \return this + public: const Vector4 &operator/=(const Vector4 &_v) + { + this->data[0] /= _v[0]; + this->data[1] /= _v[1]; + this->data[2] /= _v[2]; + this->data[3] /= _v[3]; + + return *this; + } + + /// \brief Division assignment operator + /// \remarks Performs element wise division, + /// which has limited use. + /// \param[in] _v another vector + /// \return a result vector + public: const Vector4 operator/(T _v) const + { + return Vector4(this->data[0] / _v, this->data[1] / _v, + this->data[2] / _v, this->data[3] / _v); + } + + /// \brief Division operator + /// \param[in] _v scaling factor + /// \return a vector + public: const Vector4 &operator/=(T _v) + { + this->data[0] /= _v; + this->data[1] /= _v; + this->data[2] /= _v; + this->data[3] /= _v; + + return *this; + } + + /// \brief Multiplication operator. + /// \remarks Performs element wise multiplication, + /// which has limited use. + /// \param[in] _pt another vector + /// \return result vector + public: const Vector4 operator*(const Vector4 &_pt) const + { + return Vector4(this->data[0] * _pt[0], + this->data[1] * _pt[1], + this->data[2] * _pt[2], + this->data[3] * _pt[3]); + } + + /// \brief Matrix multiplication operator. + /// \param[in] _m matrix + /// \return the vector multiplied by _m + public: const Vector4 operator*(const Matrix4 &_m) const + { + return Vector4( + this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) + + this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0), + this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) + + this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1), + this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) + + this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2), + this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) + + this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3)); + } + + /// \brief Multiplication assignment operator + /// \remarks Performs element wise multiplication, + /// which has limited use. + /// \param[in] _pt a vector + /// \return this + public: const Vector4 &operator*=(const Vector4 &_pt) + { + this->data[0] *= _pt[0]; + this->data[1] *= _pt[1]; + this->data[2] *= _pt[2]; + this->data[3] *= _pt[3]; + + return *this; + } + + /// \brief Multiplication operators + /// \param[in] _v scaling factor + /// \return a scaled vector + public: const Vector4 operator*(T _v) const + { + return Vector4(this->data[0] * _v, this->data[1] * _v, + this->data[2] * _v, this->data[3] * _v); + } + + /// \brief Scalar left multiplication operators + /// \param[in] _s the scaling factor + /// \param[in] _v the vector to scale + /// \return a scaled vector + public: friend inline const Vector4 operator*(const T _s, + const Vector4 &_v) + { + return Vector4(_v * _s); + } + + /// \brief Multiplication assignment operator + /// \param[in] _v scaling factor + /// \return this + public: const Vector4 &operator*=(T _v) + { + this->data[0] *= _v; + this->data[1] *= _v; + this->data[2] *= _v; + this->data[3] *= _v; + + return *this; + } + + /// \brief Equality test with tolerance. + /// \param[in] _v the vector to compare to + /// \param[in] _tol equality tolerance. + /// \return true if the elements of the vectors are equal within + /// the tolerence specified by _tol. + public: bool Equal(const Vector4 &_v, const T &_tol) const + { + return equal(this->data[0], _v[0], _tol) + && equal(this->data[1], _v[1], _tol) + && equal(this->data[2], _v[2], _tol) + && equal(this->data[3], _v[3], _tol); + } + + /// \brief Equal to operator + /// \param[in] _v the other vector + /// \return true if each component is equal within a + /// default tolerence (1e-6), false otherwise + public: bool operator==(const Vector4 &_v) const + { + return this->Equal(_v, static_cast(1e-6)); + } + + /// \brief Not equal to operator + /// \param[in] _pt the other vector + /// \return false if each component is equal within a + /// default tolerence (1e-6), true otherwise + public: bool operator!=(const Vector4 &_pt) const + { + return !(*this == _pt); + } + + /// \brief See if a point is finite (e.g., not nan) + /// \return true if finite, false otherwise + public: bool IsFinite() const + { + // std::isfinite works with floating point values, + // need to explicit cast to avoid ambiguity in vc++. + return std::isfinite(static_cast(this->data[0])) && + std::isfinite(static_cast(this->data[1])) && + std::isfinite(static_cast(this->data[2])) && + std::isfinite(static_cast(this->data[3])); + } + + /// \brief Array subscript operator + /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w. + /// The index is clamped to the range (0,3). + /// \return The value. + public: T &operator[](const std::size_t _index) + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; + } + + /// \brief Const-qualified array subscript operator + /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w. + /// The index is clamped to the range (0,3). + /// \return The value. + public: T operator[](const std::size_t _index) const + { + return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; + } + + /// \brief Return a mutable x value. + /// \return The x component of the vector + public: T &X() + { + return this->data[0]; + } + + /// \brief Return a mutable y value. + /// \return The y component of the vector + public: T &Y() + { + return this->data[1]; + } + + /// \brief Return a mutable z value. + /// \return The z component of the vector + public: T &Z() + { + return this->data[2]; + } + + /// \brief Return a mutable w value. + /// \return The w component of the vector + public: T &W() + { + return this->data[3]; + } + + /// \brief Get the x value. + /// \return The x component of the vector + public: T X() const + { + return this->data[0]; + } + + /// \brief Get the y value. + /// \return The y component of the vector + public: T Y() const + { + return this->data[1]; + } + + /// \brief Get the z value. + /// \return The z component of the vector + public: T Z() const + { + return this->data[2]; + } + + /// \brief Get the w value. + /// \return The w component of the vector + public: T W() const + { + return this->data[3]; + } + + /// \brief Set the x value. + /// \param[in] _v Value for the x component. + public: inline void X(const T &_v) + { + this->data[0] = _v; + } + + /// \brief Set the y value. + /// \param[in] _v Value for the y component. + public: inline void Y(const T &_v) + { + this->data[1] = _v; + } + + /// \brief Set the z value. + /// \param[in] _v Value for the z component. + public: inline void Z(const T &_v) + { + this->data[2] = _v; + } + + /// \brief Set the w value. + /// \param[in] _v Value for the w component. + public: inline void W(const T &_v) + { + this->data[3] = _v; + } + + /// \brief Less than operator. + /// \param[in] _pt Vector to compare. + /// \return True if this vector's X(), Y(), Z() or W() value is less + /// than the given vector's corresponding values. + public: bool operator<(const Vector4 &_pt) const + { + return this->data[0] < _pt[0] || this->data[1] < _pt[1] || + this->data[2] < _pt[2] || this->data[3] < _pt[3]; + } + + /// \brief Stream insertion operator + /// \param[in] _out output stream + /// \param[in] _pt Vector4 to output + /// \return The stream + public: friend std::ostream &operator<<( + std::ostream &_out, const gz::math::Vector4 &_pt) + { + _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3]; + return _out; + } + + /// \brief Stream extraction operator + /// \param[in] _in input stream + /// \param[in] _pt Vector4 to read values into + /// \return the stream + public: friend std::istream &operator>>( + std::istream &_in, gz::math::Vector4 &_pt) + { + T x, y, z, w; + + // Skip white spaces + _in.setf(std::ios_base::skipws); + _in >> x >> y >> z >> w; + if (!_in.fail()) + { + _pt.Set(x, y, z, w); + } + return _in; + } + + /// \brief Data values, 0==x, 1==y, 2==z, 3==w + private: T data[4]; + }; + + template + const Vector4 Vector4::Zero(0, 0, 0, 0); + + template + const Vector4 Vector4::One(1, 1, 1, 1); + + template const Vector4 Vector4::NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + + typedef Vector4 Vector4i; + typedef Vector4 Vector4d; + typedef Vector4 Vector4f; + } + } +} +#endif diff --git a/include/ignition/math/config.hh.in b/include/gz/math/config.hh.in similarity index 91% rename from include/ignition/math/config.hh.in rename to include/gz/math/config.hh.in index 84c001136..f395bdce7 100644 --- a/include/ignition/math/config.hh.in +++ b/include/gz/math/config.hh.in @@ -15,3 +15,12 @@ #cmakedefine IGNITION_MATH_BUILD_TYPE_PROFILE 1 #cmakedefine IGNITION_MATH_BUILD_TYPE_DEBUG 1 #cmakedefine IGNITION_MATH_BUILD_TYPE_RELEASE 1 + +namespace ignition +{ +} + +namespace gz +{ + using namespace ignition; +} diff --git a/include/ignition/math/detail/Box.hh b/include/gz/math/detail/Box.hh similarity index 96% rename from include/ignition/math/detail/Box.hh rename to include/gz/math/detail/Box.hh index 2ce5f0f9b..229d1d6ee 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/gz/math/detail/Box.hh @@ -14,10 +14,10 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_BOX_HH_ -#define IGNITION_MATH_DETAIL_BOX_HH_ +#ifndef GZ_MATH_DETAIL_BOX_HH_ +#define GZ_MATH_DETAIL_BOX_HH_ -#include "ignition/math/Triangle3.hh" +#include "gz/math/Triangle3.hh" #include #include @@ -40,7 +40,7 @@ Box::Box(T _length, T _width, T _height) ////////////////////////////////////////////////// template Box::Box(T _length, T _width, T _height, - const ignition::math::Material &_mat) + const gz::math::Material &_mat) { this->size.X(_length); this->size.Y(_width); @@ -57,7 +57,7 @@ Box::Box(const Vector3 &_size) ////////////////////////////////////////////////// template -Box::Box(const Vector3 &_size, const ignition::math::Material &_mat) +Box::Box(const Vector3 &_size, const gz::math::Material &_mat) { this->size = _size; this->material = _mat; @@ -88,14 +88,14 @@ void Box::SetSize(const math::Vector3 &_size) ////////////////////////////////////////////////// template -const ignition::math::Material &Box::Material() const +const gz::math::Material &Box::Material() const { return this->material; } ////////////////////////////////////////////////// template -void Box::SetMaterial(const ignition::math::Material &_mat) +void Box::SetMaterial(const gz::math::Material &_mat) { this->material = _mat; } diff --git a/include/ignition/math/detail/Capsule.hh b/include/gz/math/detail/Capsule.hh similarity index 96% rename from include/ignition/math/detail/Capsule.hh rename to include/gz/math/detail/Capsule.hh index ebf52b616..b3cd6d745 100644 --- a/include/ignition/math/detail/Capsule.hh +++ b/include/gz/math/detail/Capsule.hh @@ -14,13 +14,13 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_CAPSULE_HH_ -#define IGNITION_MATH_DETAIL_CAPSULE_HH_ +#ifndef GZ_MATH_DETAIL_CAPSULE_HH_ +#define GZ_MATH_DETAIL_CAPSULE_HH_ #include #include -#include -#include +#include +#include namespace ignition { diff --git a/include/ignition/math/detail/Cylinder.hh b/include/gz/math/detail/Cylinder.hh similarity index 97% rename from include/ignition/math/detail/Cylinder.hh rename to include/gz/math/detail/Cylinder.hh index ac4a96722..afb141aa5 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/gz/math/detail/Cylinder.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_CYLINDER_HH_ -#define IGNITION_MATH_DETAIL_CYLINDER_HH_ +#ifndef GZ_MATH_DETAIL_CYLINDER_HH_ +#define GZ_MATH_DETAIL_CYLINDER_HH_ namespace ignition { namespace math diff --git a/include/ignition/math/detail/Ellipsoid.hh b/include/gz/math/detail/Ellipsoid.hh similarity index 95% rename from include/ignition/math/detail/Ellipsoid.hh rename to include/gz/math/detail/Ellipsoid.hh index bc1c16f84..b84522010 100644 --- a/include/ignition/math/detail/Ellipsoid.hh +++ b/include/gz/math/detail/Ellipsoid.hh @@ -14,13 +14,13 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_ELLIPSOID_HH_ -#define IGNITION_MATH_DETAIL_ELLIPSOID_HH_ +#ifndef GZ_MATH_DETAIL_ELLIPSOID_HH_ +#define GZ_MATH_DETAIL_ELLIPSOID_HH_ #include #include -#include -#include +#include +#include namespace ignition { diff --git a/include/ignition/math/detail/Sphere.hh b/include/gz/math/detail/Sphere.hh similarity index 92% rename from include/ignition/math/detail/Sphere.hh rename to include/gz/math/detail/Sphere.hh index 740ee6741..84f6293ec 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/gz/math/detail/Sphere.hh @@ -14,10 +14,10 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_SPHERE_HH_ -#define IGNITION_MATH_DETAIL_SPHERE_HH_ +#ifndef GZ_MATH_DETAIL_SPHERE_HH_ +#define GZ_MATH_DETAIL_SPHERE_HH_ -#include "ignition/math/Sphere.hh" +#include "gz/math/Sphere.hh" namespace ignition { @@ -32,7 +32,7 @@ Sphere::Sphere(const T _radius) ////////////////////////////////////////////////// template -Sphere::Sphere(const T _radius, const ignition::math::Material &_mat) +Sphere::Sphere(const T _radius, const gz::math::Material &_mat) { this->radius = _radius; this->material = _mat; @@ -54,14 +54,14 @@ void Sphere::SetRadius(const T _radius) ////////////////////////////////////////////////// template -const ignition::math::Material &Sphere::Material() const +const gz::math::Material &Sphere::Material() const { return this->material; } ////////////////////////////////////////////////// template -void Sphere::SetMaterial(const ignition::math::Material &_mat) +void Sphere::SetMaterial(const gz::math::Material &_mat) { this->material = _mat; } diff --git a/include/ignition/math/detail/WellOrderedVector.hh b/include/gz/math/detail/WellOrderedVector.hh similarity index 92% rename from include/ignition/math/detail/WellOrderedVector.hh rename to include/gz/math/detail/WellOrderedVector.hh index 1040a9bf5..98aebfa7c 100644 --- a/include/ignition/math/detail/WellOrderedVector.hh +++ b/include/gz/math/detail/WellOrderedVector.hh @@ -14,9 +14,9 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ -#define IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ -#include +#ifndef GZ_MATH_DETAIL_WELLORDERED_VECTOR_HH_ +#define GZ_MATH_DETAIL_WELLORDERED_VECTOR_HH_ +#include namespace ignition { diff --git a/include/gz/math/graph/Edge.hh b/include/gz/math/graph/Edge.hh new file mode 100644 index 000000000..f0f777e77 --- /dev/null +++ b/include/gz/math/graph/Edge.hh @@ -0,0 +1,341 @@ +/* + * Copyright (C) 2017 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_GRAPH_EDGE_HH_ +#define GZ_MATH_GRAPH_EDGE_HH_ + +// uint64_t +#include +#include +#include +#include +#include + +#include +#include "gz/math/graph/Vertex.hh" + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { +namespace graph +{ + /// \typedef EdgeId. + /// \brief The unique Id for an edge. + using EdgeId = uint64_t; + + /// \brief Used in the Graph constructors for uniform initialization. + template + struct EdgeInitializer + { + /// \brief Constructor. + /// \param[in] _vertices The vertices of the edge. + /// \param[in] _data The data stored in the edge. + /// \param[in] _weight The weight (cost) of the edge. + EdgeInitializer(const VertexId_P &_vertices, + const E &_data = E(), + const double _weight = 1) + : vertices(_vertices), + data(_data), + weight(_weight) + { + }; + + /// \brief IDs of the vertices. + public: VertexId_P vertices; + + /// \brief User data. + public: E data; + + /// \brief The weight (cost) of the edge. + public: double weight = 1; + }; + + /// \brief Generic edge class. An edge has two ends and some constraint + /// between them. For example, a directed edge only allows traversing the + /// edge in one direction. + template + class Edge + { + /// \brief Constructor. + /// \param[in] _vertices The vertices of the edge. + /// \param[in] _data The data stored in the edge. + /// \param[in] _weight The weight (cost) of the edge. + /// \param[in] _id Optional unique id. + public: explicit Edge(const VertexId_P &_vertices, + const E &_data, + const double _weight, + const EdgeId &_id = kNullId) + : id(_id), + vertices(_vertices), + data(_data), + weight(_weight) + { + } + + /// \brief Get the edge Id. + /// \return The edge Id. + public: EdgeId Id() const + { + return this->id; + } + + /// \brief Get the two vertices contained in the edge. + /// \return The two vertices contained in the edge. + public: VertexId_P Vertices() const + { + if (!this->Valid()) + return {kNullId, kNullId}; + + return this->vertices; + } + + /// \brief Get a non-mutable reference to the user data stored in the edge + /// \return The non-mutable reference to the user data stored in the edge. + public: const E &Data() const + { + return this->data; + } + + /// \brief Get a mutable reference to the user data stored in the edge. + /// \return The mutable reference to the user data stored in the edge. + public: E &Data() + { + return this->data; + } + + /// \brief The cost of traversing the _from end to the other end of the + /// edge. + /// \return The cost. + public: double Weight() const + { + return this->weight; + } + + /// \brief Set the cost of the edge. + /// \param[in] _newWeight The new cost. + public: void SetWeight(const double _newWeight) + { + this->weight = _newWeight; + } + + /// \brief Get the destination end that is reachable from a source end of + /// an edge. + /// + /// E.g.: Let's assume that we have an undirected edge (e1) with ends + /// (v1) and (v2): (v1)--(v2). The operation e1.From(v1) returns (v2). + /// The operation e1.From(v2) returns (v1). + /// + /// E.g.: Let's assume that we have a directed edge (e2) with the tail end + /// (v1) and the head end (v2): (v1)->(v2). The operation e2.From(v1) + /// returns (v2). The operation e2.From(v2) returns kNullId. + /// + /// \param[in] _from Source vertex. + /// \return The other vertex of the edge reachable from the "_from" + /// vertex or kNullId otherwise. + public: virtual VertexId From(const VertexId &_from) const = 0; + + /// \brief Get the source end that can reach the destination end of + /// an edge. + /// + /// E.g.: Let's assume that we have an undirected edge (e1) with ends + /// (v1) and (v2): (v1)--(v2). The operation e1.To(v1) returns (v2). + /// The operation e1.To(v2) returns (v1). + /// + /// E.g.: Let's assume that we have a directed edge (e2) with the tail end + /// (v1) and the head end (v2): (v1)->(v2). The operation e2.To(v1) + /// returns kNullId. The operation e2.To(v2) returns v1. + /// + /// \param[in] _to Destination vertex. + /// \return The other vertex of the edge that can reach "_to" + /// vertex or kNullId otherwise. + public: virtual VertexId To(const VertexId &_to) const = 0; + + /// \brief An edge is considered valid when its id is not kNullId. + /// \return Whether the edge is valid or not. + public: bool Valid() const + { + return this->id != kNullId; + } + + /// \brief Unique edge Id. + private: EdgeId id = kNullId; + + /// \brief The set of Ids of the two vertices. + private: VertexId_P vertices; + + /// \brief User data. + private: E data; + + /// \brief The weight (cost) of the edge. By default, the cost of an edge + /// is 1.0 . + private: double weight = 1.0; + }; + + /// \def EdgeId_S + /// \brief A set of edge Ids. + using EdgeId_S = std::set; + + /// \def EdgeRef_M + /// \brief A map of edges. The key is the edge Id. The value is a reference + /// to the edge. + template + using EdgeRef_M = std::map>; + + /// \brief An undirected edge represents a connection between two vertices. + /// The connection is bidirectional, it's possible to traverse the edge + /// in both directions. + template + class UndirectedEdge : public Edge + { + /// \brief An invalid undirected edge. + public: static UndirectedEdge NullEdge; + + /// \brief Constructor. + /// \param[in] _vertices The vertices of the edge. + /// \param[in] _data The data stored in the edge. + /// \param[in] _weight The weight (cost) of the edge. + /// \param[in] _id Optional unique id. + public: explicit UndirectedEdge(const VertexId_P &_vertices, + const E &_data, + const double _weight, + const EdgeId &_id = kNullId) + : Edge(_vertices, _data, _weight, _id) + { + } + + // Documentation inherited. + public: VertexId From(const VertexId &_from) const override + { + if (!this->Valid()) + return kNullId; + + if (this->Vertices().first != _from && this->Vertices().second != _from) + return kNullId; + + if (this->Vertices().first == _from) + return this->Vertices().second; + + return this->Vertices().first; + } + + // Documentation inherited. + public: VertexId To(const VertexId &_to) const override + { + return this->From(_to); + } + + /// \brief Stream insertion operator. The output uses DOT graph + /// description language. + /// \param[out] _out The output stream. + /// \param[in] _e Edge to write to the stream. + /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). + public: friend std::ostream &operator<<(std::ostream &_out, + const UndirectedEdge &_e) + { + auto vertices = _e.Vertices(); + _out << " " << vertices.first << " -- " << vertices.second + << " [label=" << _e.Weight() << "];" << std::endl; + return _out; + } + }; + + /// \brief An invalid undirected edge. + template + UndirectedEdge UndirectedEdge::NullEdge( + VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); + + /// \brief A directed edge represents a connection between two vertices. + /// The connection is unidirectional, it's only possible to traverse the edge + /// in one direction (from the tail to the head). + template + class DirectedEdge : public Edge + { + /// \brief An invalid directed edge. + public: static DirectedEdge NullEdge; + + /// \brief Constructor. + /// \param[in] _vertices The vertices of the edge. + /// \param[in] _data The data stored in the edge. + /// \param[in] _weight The weight (cost) of the edge. + /// \param[in] _id Optional unique id. + public: explicit DirectedEdge(const VertexId_P &_vertices, + const E &_data, + const double _weight, + const EdgeId &_id = kNullId) + : Edge(_vertices, _data, _weight, _id) + { + } + + /// \brief Get the Id of the tail vertex in this edge. + /// \return An id of the tail vertex in this edge. + /// \sa Head() + public: VertexId Tail() const + { + return this->Vertices().first; + } + + /// \brief Get the Id of the head vertex in this edge. + /// \return An id of the head vertex in this edge. + /// \sa Tail() + public: VertexId Head() const + { + return this->Vertices().second; + } + + // Documentation inherited. + public: VertexId From(const VertexId &_from) const override + { + if (_from != this->Tail()) + return kNullId; + + return this->Head(); + } + + // Documentation inherited. + public: VertexId To(const VertexId &_to) const override + { + if (_to != this->Head()) + return kNullId; + + return this->Tail(); + } + + /// \brief Stream insertion operator. The output uses DOT graph + /// description language. + /// \param[out] _out The output stream. + /// \param[in] _e Edge to write to the stream. + /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). + public: friend std::ostream &operator<<(std::ostream &_out, + const DirectedEdge &_e) + { + _out << " " << _e.Tail() << " -> " << _e.Head() + << " [label=" << _e.Weight() << "];" << std::endl; + return _out; + } + }; + + /// \brief An invalid directed edge. + template + DirectedEdge DirectedEdge::NullEdge( + VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); +} +} +} +} +#endif diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh new file mode 100644 index 000000000..05b5c9414 --- /dev/null +++ b/include/gz/math/graph/Graph.hh @@ -0,0 +1,809 @@ +/* + * Copyright (C) 2017 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_GRAPH_GRAPH_HH_ +#define GZ_MATH_GRAPH_GRAPH_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "gz/math/graph/Edge.hh" +#include "gz/math/graph/Vertex.hh" + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { +namespace graph +{ + /// \brief A generic graph class. + /// Both vertices and edges can store user information. A vertex could be + /// created passing a custom Id if needed, otherwise it will be choosen + /// internally. The vertices also have a name that could be reused among + /// other vertices if needed. This class supports the use of different edge + /// types (e.g. directed or undirected edges). + /// + /// Example directed graph + // + /// \code{.cpp} + /// + /// // Create a directed graph that is capable of storing integer data in the + /// // vertices and double data on the edges. + /// gz::math::graph::DirectedGraph graph( + /// // Create the vertices, with default data and vertex ids. + /// { + /// {"vertex1"}, {"vertex2"}, {"vertex3"} + /// }, + /// // Create the edges, with default data and weight values. + /// { + /// // Edge from vertex 0 to vertex 1. Each number refers to a vertex id. + /// // Vertex ids start from zero. + /// {{0, 1}}, {{1, 2}} + /// }); + /// + /// // You can assign data to vertices. + /// gz::math::graph::DirectedGraph graph2( + /// // Create the vertices, with custom data and default vertex ids. + /// { + /// {"vertex1", 1}, {"vertex2", 2}, {"vertex3", 10} + /// }, + /// // Create the edges, with default data and weight values. + /// { + /// // Edge from vertex 0 to vertex 1. Each number refers to a vertex id + /// // specified above. + /// {{0, 2}}, {{1, 2}} + /// }); + /// + /// + /// // It's also possible to specify vertex ids. + /// gz::math::graph::DirectedGraph graph3( + /// // Create the vertices with custom data and vertex ids. + /// { + /// {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} + /// }, + /// // Create the edges, with custom data and default weight values. + /// { + /// {{2, 3}, 6.3}, {{3, 4}, 4.2} + /// }); + /// + /// // Finally, you can also assign weights to the edges. + /// gz::math::graph::DirectedGraph graph4( + /// // Create the vertices with custom data and vertex ids. + /// { + /// {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} + /// }, + /// // Create the edges, with custom data and default weight values + /// { + /// {{2, 3}, 6.3, 1.1}, {{3, 4}, 4.2, 2.3} + /// }); + /// \endcode + template + class Graph + { + /// \brief Default constructor. + public: Graph() = default; + + /// \brief Constructor. + /// \param[in] _vertices Collection of vertices. + /// \param[in] _edges Collection of edges. + public: Graph(const std::vector> &_vertices, + const std::vector> &_edges) + { + // Add all vertices. + for (auto const &v : _vertices) + { + if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid()) + { + std::cerr << "Invalid vertex with Id [" << v.Id() << "]. Ignoring." + << std::endl; + } + } + + // Add all edges. + for (auto const &e : _edges) + { + if (!this->AddEdge(e.vertices, e.data, e.weight).Valid()) + std::cerr << "Ignoring edge" << std::endl; + } + } + + /// \brief Add a new vertex to the graph. + /// \param[in] _name Name of the vertex. It doesn't have to be unique. + /// \param[in] _data Data to be stored in the vertex. + /// \param[in] _id Optional Id to be used for this vertex. This Id must + /// be unique. + /// \return A reference to the new vertex. + public: Vertex &AddVertex(const std::string &_name, + const V &_data, + const VertexId &_id = kNullId) + { + auto id = _id; + + // The user didn't provide an Id, we generate it. + if (id == kNullId) + { + id = this->NextVertexId(); + + // No space for new Ids. + if (id == kNullId) + { + std::cerr << "[Graph::AddVertex()] The limit of vertices has been " + << "reached. Ignoring vertex." << std::endl; + return Vertex::NullVertex; + } + } + + // Create the vertex. + auto ret = this->vertices.insert( + std::make_pair(id, Vertex(_name, _data, id))); + + // The Id already exists. + if (!ret.second) + { + std::cerr << "[Graph::AddVertex()] Repeated vertex [" << id << "]" + << std::endl; + return Vertex::NullVertex; + } + + // Link the vertex with an empty list of edges. + this->adjList[id] = EdgeId_S(); + + // Update the map of names. + this->names.insert(std::make_pair(_name, id)); + + return ret.first->second; + } + + /// \brief The collection of all vertices in the graph. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. + public: const VertexRef_M Vertices() const + { + VertexRef_M res; + for (auto const &v : this->vertices) + res.emplace(std::make_pair(v.first, std::cref(v.second))); + + return res; + } + + /// \brief The collection of all vertices in the graph with name == _name. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. + public: const VertexRef_M Vertices(const std::string &_name) const + { + VertexRef_M res; + for (auto const &vertex : this->vertices) + { + if (vertex.second.Name() == _name) + res.emplace(std::make_pair(vertex.first, std::cref(vertex.second))); + } + + return res; + } + + /// \brief Add a new edge to the graph. + /// \param[in] _vertices The set of Ids of the two vertices. + /// \param[in] _data User data. + /// \param[in] _weight Edge weight. + /// \return Reference to the new edge created or NullEdge if the + /// edge was not created (e.g. incorrect vertices). + public: EdgeType &AddEdge(const VertexId_P &_vertices, + const E &_data, + const double _weight = 1.0) + { + auto id = this->NextEdgeId(); + + // No space for new Ids. + if (id == kNullId) + { + std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. " + << "Ignoring edge." << std::endl; + return EdgeType::NullEdge; + } + + EdgeType newEdge(_vertices, _data, _weight, id); + return this->LinkEdge(std::move(newEdge)); + } + + /// \brief Links an edge to the graph. This function verifies that the + /// edge's two vertices exist in the graph, copies the edge into the + /// graph's internal data structure, and returns a reference to this + /// new edge. + /// \param[in] _edge An edge to copy into the graph. + /// \return A reference to the new edge. + public: EdgeType &LinkEdge(const EdgeType &_edge) + { + auto edgeVertices = _edge.Vertices(); + + // Sanity check: Both vertices should exist. + for (auto const &v : {edgeVertices.first, edgeVertices.second}) + { + if (this->vertices.find(v) == this->vertices.end()) + return EdgeType::NullEdge; + } + + // Link the new edge. + for (auto const &v : {edgeVertices.first, edgeVertices.second}) + { + if (v != kNullId) + { + auto vertexIt = this->adjList.find(v); + assert(vertexIt != this->adjList.end()); + vertexIt->second.insert(_edge.Id()); + } + } + + auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge)); + + // Return the new edge. + return ret.first->second; + } + + /// \brief The collection of all edges in the graph. + /// \return A map of edges, where keys are Ids and values are references + /// to the edges. + public: const EdgeRef_M Edges() const + { + EdgeRef_M res; + for (auto const &edge : this->edges) + { + res.emplace(std::make_pair(edge.first, std::cref(edge.second))); + } + + return res; + } + + /// \brief Get all vertices that are directly connected with one edge + /// from a given vertex. In other words, this function will return + /// child vertices of the given vertex (all vertices from the given + /// vertex). E.g. j is adjacent from i (the given vertex) if there is an + /// edge (i->j). + /// + /// In an undirected graph, the result of this function will match + /// the result provided by AdjacentsTo. + /// + /// \param[in] _vertex The Id of the vertex from which adjacent + /// vertices will be returned. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. This is the set of adjacent vertices. + /// An empty map will be returned when the _vertex is not found in the + /// graph. + public: VertexRef_M AdjacentsFrom(const VertexId &_vertex) const + { + VertexRef_M res; + + // Make sure the vertex exists + auto vertexIt = this->adjList.find(_vertex); + if (vertexIt == this->adjList.end()) + return res; + + for (auto const &edgeId : vertexIt->second) + { + const auto &edge = this->EdgeFromId(edgeId); + auto neighborVertexId = edge.From(_vertex); + if (neighborVertexId != kNullId) + { + const auto &neighborVertex = this->VertexFromId(neighborVertexId); + res.emplace( + std::make_pair(neighborVertexId, std::cref(neighborVertex))); + } + } + + return res; + } + + /// \brief Get all vertices that are directly connected with one edge + /// from a given vertex. In other words, this function will return + /// child vertices of the given vertex (all vertices from the given + /// vertex). E.g. j is adjacent from i (the given vertex) if there is an + /// edge (i->j). + /// + /// In an undirected graph, the result of this function will match + /// the result provided by AdjacentsTo. + /// + /// \param[in] _vertex The Id of the vertex from which adjacent + /// vertices will be returned. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. This is the set of adjacent vertices. + /// An empty map will be returned when the _vertex is not found in the + /// graph. + public: VertexRef_M AdjacentsFrom(const Vertex &_vertex) const + { + return this->AdjacentsFrom(_vertex.Id()); + } + + /// \brief Get all vertices that are directly connected with one edge + /// to a given vertex. In other words, this function will return + /// child vertices of the given vertex (all vertices from the given + /// vertex). + /// + /// In an undirected graph, the result of this function will match + /// the result provided by AdjacentsFrom. + /// + /// E.g. i is adjacent to j (the given vertex) if there is an + /// edge (i->j). + /// \param[in] _vertex The Id of the vertex to check adjacentsTo. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. An empty map is returned if the + /// _vertex is not present in this graph, or when there are no + /// adjacent vertices. + public: VertexRef_M AdjacentsTo(const VertexId &_vertex) const + { + auto incidentEdges = this->IncidentsTo(_vertex); + + VertexRef_M res; + for (auto const &incidentEdgeRef : incidentEdges) + { + const auto &incidentEdgeId = incidentEdgeRef.first; + const auto &incidentEdge = this->EdgeFromId(incidentEdgeId); + const auto &neighborVertexId = incidentEdge.To(_vertex); + const auto &neighborVertex = this->VertexFromId(neighborVertexId); + res.emplace( + std::make_pair(neighborVertexId, std::cref(neighborVertex))); + } + + return res; + } + + /// \brief Get all vertices that are directly connected with one edge + /// to a given vertex. In other words, this function will return + /// child vertices of the given vertex (all vertices from the given + /// vertex). + /// + /// In an undirected graph, the result of this function will match + /// the result provided by AdjacentsFrom. + /// + /// E.g. i is adjacent to j (the given vertex) if there is an + /// edge (i->j). + /// \param[in] _vertex The vertex to check adjacentsTo. + /// \return A map of vertices, where keys are Ids and values are + /// references to the vertices. An empty map is returned if the + /// _vertex is not present in this graph, or when there are no + /// adjacent vertices. + public: VertexRef_M AdjacentsTo(const Vertex &_vertex) const + { + return this->AdjacentsTo(_vertex.Id()); + } + + /// \brief Get the number of edges incident to a vertex. + /// \param[in] _vertex The vertex Id. + /// \return The number of edges incidents to a vertex. + public: size_t InDegree(const VertexId &_vertex) const + { + return this->IncidentsTo(_vertex).size(); + } + + /// \brief Get the number of edges incident to a vertex. + /// \param[in] _vertex The vertex. + /// \return The number of edges incidents to a vertex. + public: size_t InDegree(const Vertex &_vertex) const + { + return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size(); + } + + /// \brief Get the number of edges incident from a vertex. + /// \param[in] _vertex The vertex Id. + /// \return The number of edges incidents from a vertex. + public: size_t OutDegree(const VertexId &_vertex) const + { + return this->IncidentsFrom(_vertex).size(); + } + + /// \brief Get the number of edges incident from a vertex. + /// \param[in] _vertex The vertex. + /// \return The number of edges incidents from a vertex. + public: size_t OutDegree(const Vertex &_vertex) const + { + return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size(); + } + + /// \brief Get the set of outgoing edges from a given vertex. + /// \param[in] _vertex Id of the vertex. + /// \return A map of edges, where keys are Ids and values are + /// references to the edges. An empty map is returned when the provided + /// vertex does not exist, or when there are no outgoing edges. + public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) + const + { + EdgeRef_M res; + + const auto &adjIt = this->adjList.find(_vertex); + if (adjIt == this->adjList.end()) + return res; + + const auto &edgeIds = adjIt->second; + for (auto const &edgeId : edgeIds) + { + const auto &edge = this->EdgeFromId(edgeId); + if (edge.From(_vertex) != kNullId) + res.emplace(std::make_pair(edge.Id(), std::cref(edge))); + } + + return res; + } + + /// \brief Get the set of outgoing edges from a given vertex. + /// \param[in] _vertex The vertex. + /// \return A map of edges, where keys are Ids and values are + /// references to the edges. An empty map is returned when the provided + /// vertex does not exist, or when there are no outgoing edges. + public: const EdgeRef_M IncidentsFrom( + const Vertex &_vertex) const + { + return this->IncidentsFrom(_vertex.Id()); + } + + /// \brief Get the set of incoming edges to a given vertex. + /// \param[in] _vertex Id of the vertex. + /// \return A map of edges, where keys are Ids and values are + /// references to the edges. An empty map is returned when the provided + /// vertex does not exist, or when there are no incoming edges. + public: const EdgeRef_M IncidentsTo( + const VertexId &_vertex) const + { + EdgeRef_M res; + + const auto &adjIt = this->adjList.find(_vertex); + if (adjIt == this->adjList.end()) + return res; + + const auto &edgeIds = adjIt->second; + for (auto const &edgeId : edgeIds) + { + const auto &edge = this->EdgeFromId(edgeId); + if (edge.To(_vertex) != kNullId) + res.emplace(std::make_pair(edge.Id(), std::cref(edge))); + } + + return res; + } + + /// \brief Get the set of incoming edges to a given vertex. + /// \param[in] _vertex The vertex. + /// \return A map of edges, where keys are Ids and values are + /// references to the edges. An empty map is returned when the provided + /// vertex does not exist, or when there are no incoming edges. + public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) + const + { + return this->IncidentsTo(_vertex.Id()); + } + + /// \brief Get whether the graph is empty. + /// \return True when there are no vertices in the graph or + /// false otherwise. + public: bool Empty() const + { + return this->vertices.empty(); + } + + /// \brief Remove an existing vertex from the graph. + /// \param[in] _vertex Id of the vertex to be removed. + /// \return True when the vertex was removed or false otherwise. + public: bool RemoveVertex(const VertexId &_vertex) + { + auto vIt = this->vertices.find(_vertex); + if (vIt == this->vertices.end()) + return false; + + std::string name = vIt->second.Name(); + + // Remove incident edges. + auto incidents = this->IncidentsTo(_vertex); + for (auto edgePair : incidents) + this->RemoveEdge(edgePair.first); + + // Remove all outgoing edges. + incidents = this->IncidentsFrom(_vertex); + for (auto edgePair : incidents) + this->RemoveEdge(edgePair.first); + + // Remove the vertex (key) from the adjacency list. + this->adjList.erase(_vertex); + + // Remove the vertex. + this->vertices.erase(_vertex); + + // Get an iterator to all vertices sharing name. + auto iterPair = this->names.equal_range(name); + for (auto it = iterPair.first; it != iterPair.second; ++it) + { + if (it->second == _vertex) + { + this->names.erase(it); + break; + } + } + + return true; + } + + /// \brief Remove an existing vertex from the graph. + /// \param[in] _vertex The vertex to be removed. + /// \return True when the vertex was removed or false otherwise. + public: bool RemoveVertex(Vertex &_vertex) + { + return this->RemoveVertex(_vertex.Id()); + } + + /// \brief Remove all vertices with name == _name. + /// \param[in] _name Name of the vertices to be removed. + /// \return The number of vertices removed. + public: size_t RemoveVertices(const std::string &_name) + { + size_t numVertices = this->names.count(_name); + size_t result = 0; + for (size_t i = 0; i < numVertices; ++i) + { + auto iter = this->names.find(_name); + if (this->RemoveVertex(iter->second)) + ++result; + } + + return result; + } + + /// \brief Remove an existing edge from the graph. After the removal, it + /// won't be possible to reach any of the vertices from the edge, unless + /// there are other edges that connect the to vertices. + /// \param[in] _edge Id of the edge to be removed. + /// \return True when the edge was removed or false otherwise. + public: bool RemoveEdge(const EdgeId &_edge) + { + auto edgeIt = this->edges.find(_edge); + if (edgeIt == this->edges.end()) + return false; + + auto edgeVertices = edgeIt->second.Vertices(); + + // Unlink the edge. + for (auto const &v : {edgeVertices.first, edgeVertices.second}) + { + if (edgeIt->second.From(v) != kNullId) + { + auto vertex = this->adjList.find(v); + assert(vertex != this->adjList.end()); + vertex->second.erase(_edge); + } + } + + this->edges.erase(_edge); + + return true; + } + + /// \brief Remove an existing edge from the graph. After the removal, it + /// won't be possible to reach any of the vertices from the edge, unless + /// there are other edges that connect the to vertices. + /// \param[in] _edge The edge to be removed. + /// \return True when the edge was removed or false otherwise. + public: bool RemoveEdge(EdgeType &_edge) + { + return this->RemoveEdge(_edge.Id()); + } + + /// \brief Get a reference to a vertex using its Id. + /// \param[in] _id The Id of the vertex. + /// \return A reference to the vertex with Id = _id or NullVertex if + /// not found. + public: const Vertex &VertexFromId(const VertexId &_id) const + { + auto iter = this->vertices.find(_id); + if (iter == this->vertices.end()) + return Vertex::NullVertex; + + return iter->second; + } + + /// \brief Get a mutable reference to a vertex using its Id. + /// \param[in] _id The Id of the vertex. + /// \return A mutable reference to the vertex with Id = _id or NullVertex + /// if not found. + public: Vertex &VertexFromId(const VertexId &_id) + { + auto iter = this->vertices.find(_id); + if (iter == this->vertices.end()) + return Vertex::NullVertex; + + return iter->second; + } + + /// \brief Get a reference to an edge based on two vertices. A NullEdge + /// object reference is returned if an edge with the two vertices is not + /// found. If there are multiple edges that match the provided vertices, + /// then first is returned. + /// \param[in] _sourceId Source vertex Id. + /// \param[in] _destId Destination vertex Id. + /// \return A reference to the first edge found, or NullEdge if + /// not found. + public: const EdgeType &EdgeFromVertices( + const VertexId _sourceId, const VertexId _destId) const + { + // Get the adjacency iterator for the source vertex. + const typename std::map::const_iterator &adjIt = + this->adjList.find(_sourceId); + + // Quit early if there is no adjacency entry + if (adjIt == this->adjList.end()) + return EdgeType::NullEdge; + + // Loop over the edges in the source vertex's adjacency list + for (std::set::const_iterator edgIt = adjIt->second.begin(); + edgIt != adjIt->second.end(); ++edgIt) + { + // Get an iterator to the actual edge + const typename std::map::const_iterator edgeIter = + this->edges.find(*edgIt); + + // Check if the edge has the correct source and destination. + if (edgeIter != this->edges.end() && + edgeIter->second.From(_sourceId) == _destId) + { + assert(edgeIter->second.To(_destId) == _sourceId); + return edgeIter->second; + } + } + + return EdgeType::NullEdge; + } + + /// \brief Get a reference to an edge using its Id. + /// \param[in] _id The Id of the edge. + /// \return A reference to the edge with Id = _id or NullEdge if + /// not found. + public: const EdgeType &EdgeFromId(const EdgeId &_id) const + { + auto iter = this->edges.find(_id); + if (iter == this->edges.end()) + return EdgeType::NullEdge; + + return iter->second; + } + + /// \brief Stream insertion operator. The output uses DOT graph + /// description language. + /// \param[out] _out The output stream. + /// \param[in] _g Graph to write to the stream. + /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). + public: template + friend std::ostream &operator<<(std::ostream &_out, + const Graph &_g); + + /// \brief Get an available Id to be assigned to a new vertex. + /// \return The next available Id or kNullId if there aren't ids available. + private: VertexId &NextVertexId() + { + while (this->vertices.find(this->nextVertexId) != this->vertices.end() + && this->nextVertexId < MAX_UI64) + { + ++this->nextVertexId; + } + + return this->nextVertexId; + } + + /// \brief Get an available Id to be assigned to a new edge. + /// \return The next available Id or kNullId if there aren't ids available. + private: VertexId &NextEdgeId() + { + while (this->edges.find(this->nextEdgeId) != this->edges.end() && + this->nextEdgeId < MAX_UI64) + { + ++this->nextEdgeId; + } + + return this->nextEdgeId; + } + + /// \brief The next vertex Id to be assigned to a new vertex. + protected: VertexId nextVertexId = 0u; + + /// \brief The next edge Id to be assigned to a new edge. + protected: VertexId nextEdgeId = 0u; + + /// \brief The set of vertices. + private: std::map> vertices; + + /// \brief The set of edges. + private: std::map edges; + + /// \brief The adjacency list. + /// A map where the keys are vertex Ids. For each vertex (v) + /// with id (vId), the map value contains a set of edge Ids. Each of + /// the edges (e) with Id (eId) represents a connected path from (v) to + /// another vertex via (e). + private: std::map adjList; + + /// \brief Association between names and vertices curently used. + private: std::multimap names; + }; + + ///////////////////////////////////////////////// + /// Partial template specification for undirected edges. + template + std::ostream &operator<<(std::ostream &_out, + const Graph> &_g) + { + _out << "graph {" << std::endl; + + // All vertices with the name and Id as a "label" attribute. + for (auto const &vertexMap : _g.Vertices()) + { + auto vertex = vertexMap.second.get(); + _out << vertex; + } + + // All edges. + for (auto const &edgeMap : _g.Edges()) + { + auto edge = edgeMap.second.get(); + _out << edge; + } + + _out << "}" << std::endl; + + return _out; + } + + ///////////////////////////////////////////////// + /// Partial template specification for directed edges. + template + std::ostream &operator<<(std::ostream &_out, + const Graph> &_g) + { + _out << "digraph {" << std::endl; + + // All vertices with the name and Id as a "label" attribute. + for (auto const &vertexMap : _g.Vertices()) + { + auto vertex = vertexMap.second.get(); + _out << vertex; + } + + // All edges. + for (auto const &edgeMap : _g.Edges()) + { + auto edge = edgeMap.second.get(); + _out << edge; + } + + _out << "}" << std::endl; + + return _out; + } + + /// \def UndirectedGraph + /// \brief An undirected graph. + template + using UndirectedGraph = Graph>; + + /// \def DirectedGraph + /// \brief A directed graph. + template + using DirectedGraph = Graph>; +} +} +} +} +#endif diff --git a/include/gz/math/graph/GraphAlgorithms.hh b/include/gz/math/graph/GraphAlgorithms.hh new file mode 100644 index 000000000..2d6e55703 --- /dev/null +++ b/include/gz/math/graph/GraphAlgorithms.hh @@ -0,0 +1,359 @@ +/* + * Copyright (C) 2017 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_GRAPH_GRAPHALGORITHMS_HH_ +#define GZ_MATH_GRAPH_GRAPHALGORITHMS_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "gz/math/graph/Graph.hh" +#include "gz/math/Helpers.hh" + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { +namespace graph +{ + /// \typedef CostInfo. + /// \brief Used in Dijkstra. For a given source vertex, this pair represents + /// the cost (first element) to reach a destination vertex (second element). + using CostInfo = std::pair; + + /// \brief Breadth first sort (BFS). + /// Starting from the vertex == _from, it traverses the graph exploring the + /// neighbors first, before moving to the next level neighbors. + /// \param[in] _graph A graph. + /// \param[in] _from The starting vertex. + /// \return The vector of vertices Ids traversed in a breadth first manner. + template + std::vector BreadthFirstSort(const Graph &_graph, + const VertexId &_from) + { + // Create an auxiliary graph, where the data is just a boolean value that + // stores whether the vertex has been visited or not. + Graph visitorGraph; + + // Copy the vertices (just the Id). + for (auto const &v : _graph.Vertices()) + visitorGraph.AddVertex("", false, v.first); + + // Copy the edges (without data). + for (auto const &e : _graph.Edges()) + visitorGraph.AddEdge(e.second.get().Vertices(), E()); + + std::vector visited; + std::list pending = {_from}; + + while (!pending.empty()) + { + auto vId = pending.front(); + pending.pop_front(); + + // If the vertex has been visited, skip. + auto &vertex = visitorGraph.VertexFromId(vId); + if (vertex.Data()) + continue; + + visited.push_back(vId); + vertex.Data() = true; + + // Add more vertices to visit if they haven't been visited yet. + auto adjacents = visitorGraph.AdjacentsFrom(vId); + for (auto const &adj : adjacents) + { + vId = adj.first; + auto &v = adj.second.get(); + if (!v.Data()) + pending.push_back(vId); + } + } + + return visited; + } + + /// \brief Depth first sort (DFS). + /// Starting from the vertex == _from, it visits the graph as far as + /// possible along each branch before backtracking. + /// \param[in] _graph A graph. + /// \param[in] _from The starting vertex. + /// \return The vector of vertices Ids visited in a depth first manner. + template + std::vector DepthFirstSort(const Graph &_graph, + const VertexId &_from) + { + // Create an auxiliary graph, where the data is just a boolean value that + // stores whether the vertex has been visited or not. + Graph visitorGraph; + + // Copy the vertices (just the Id). + for (auto const &v : _graph.Vertices()) + visitorGraph.AddVertex("", false, v.first); + + // Copy the edges (without data). + for (auto const &e : _graph.Edges()) + visitorGraph.AddEdge(e.second.get().Vertices(), E()); + + std::vector visited; + std::stack pending({_from}); + + while (!pending.empty()) + { + auto vId = pending.top(); + pending.pop(); + + // If the vertex has been visited, skip. + auto &vertex = visitorGraph.VertexFromId(vId); + if (vertex.Data()) + continue; + + visited.push_back(vId); + vertex.Data() = true; + + // Add more vertices to visit if they haven't been visited yet. + auto adjacents = visitorGraph.AdjacentsFrom(vId); + for (auto const &adj : adjacents) + { + vId = adj.first; + auto &v = adj.second.get(); + if (!v.Data()) + pending.push(vId); + } + } + + return visited; + } + + /// \brief Dijkstra algorithm. + /// Find the shortest path between the vertices in a graph. + /// If only a graph and a source vertex is provided, the algorithm will + /// find shortest paths from the source vertex to all other vertices in the + /// graph. If an additional destination vertex is provided, the algorithm + /// will stop when the shortest path is found between the source and + /// destination vertex. + /// \param[in] _graph A graph. + /// \param[in] _from The starting vertex. + /// \param[in] _to Optional destination vertex. + /// \return A map where the keys are the destination vertices. For each + /// destination, the value is another pair, where the key is the shortest + /// cost from the origin vertex. The value is the previous neighbor Id in the + /// shortest path. + /// Note: In the case of providing a destination vertex, only the entry in the + /// map with key = _to should be used. The rest of the map may contain + /// incomplete information. If you want all shortest paths to all other + /// vertices, please remove the destination vertex. + /// If the source or destination vertex don't exist, the function will return + /// an empty map. + /// + /// E.g.: Given the following undirected graph, g, with five vertices: + /// + /// (6) | + /// 0-------1 | + /// | /|\ | + /// | / | \(5) | + /// | (2)/ | \ | + /// | / | 2 | + /// (1)| / (2)| / | + /// | / | /(5) | + /// |/ |/ | + /// 3-------4 | + /// (1) | + /// + /// This is the resut of Dijkstra(g, 0): + /// + /// \code + /// ================================ + /// | Dst | Cost | Previous vertex | + /// ================================ + /// | 0 | 0 | 0 | + /// | 1 | 3 | 3 | + /// | 2 | 7 | 4 | + /// | 3 | 1 | 0 | + /// | 4 | 2 | 3 | + /// ================================ + /// \endcode + /// + /// This is the result of Dijkstra(g, 0, 3): + /// + /// \code + /// ================================ + /// | Dst | Cost | Previous vertex | + /// ================================ + /// | 0 | 0 | 0 | + /// | 1 |ignore| ignore | + /// | 2 |ignore| ignore | + /// | 3 | 1 | 0 | + /// | 4 |ignore| ignore | + /// ================================ + /// \endcode + /// + template + std::map Dijkstra(const Graph &_graph, + const VertexId &_from, + const VertexId &_to = kNullId) + { + auto allVertices = _graph.Vertices(); + + // Sanity check: The source vertex should exist. + if (allVertices.find(_from) == allVertices.end()) + { + std::cerr << "Vertex [" << _from << "] Not found" << std::endl; + return {}; + } + + // Sanity check: The destination vertex should exist (if used). + if (_to != kNullId && + allVertices.find(_to) == allVertices.end()) + { + std::cerr << "Vertex [" << _from << "] Not found" << std::endl; + return {}; + } + + // Store vertices that are being preprocessed. + std::priority_queue, std::greater> pq; + + // Create a map for distances and next neightbor and initialize all + // distances as infinite. + std::map dist; + for (auto const &v : allVertices) + { + auto id = v.first; + dist[id] = std::make_pair(MAX_D, kNullId); + } + + // Insert _from in the priority queue and initialize its distance as 0. + pq.push(std::make_pair(0.0, _from)); + dist[_from] = std::make_pair(0.0, _from); + + while (!pq.empty()) + { + // This is the minimum distance vertex. + VertexId u = pq.top().second; + + // Shortcut: Destination vertex found, exiting. + if (_to != kNullId && _to == u) + break; + + pq.pop(); + + for (auto const &edgePair : _graph.IncidentsFrom(u)) + { + const auto &edge = edgePair.second.get(); + const auto &v = edge.From(u); + double weight = edge.Weight(); + + // If there is shorted path to v through u. + if (dist[v].first > dist[u].first + weight) + { + // Updating distance of v. + dist[v] = std::make_pair(dist[u].first + weight, u); + pq.push(std::make_pair(dist[v].first, v)); + } + } + } + + return dist; + } + + /// \brief Calculate the connected components of an undirected graph. + /// A connected component of an undirected graph is a subgraph in which any + /// two vertices are connected to each other by paths, and which is connected + /// to no additional vertices in the supergraph. + /// \sa https://en.wikipedia.org/wiki/Connected_component_(graph_theory) + /// \param[in] _graph A graph. + /// \return A vector of graphs. Each element of the graph is a component + /// (subgraph) of the original graph. + template + std::vector> ConnectedComponents( + const UndirectedGraph &_graph) + { + std::map visited; + unsigned int componentCount = 0; + + for (auto const &v : _graph.Vertices()) + { + if (visited.find(v.first) == visited.end()) + { + auto component = BreadthFirstSort(_graph, v.first); + for (auto const &vId : component) + visited[vId] = componentCount; + ++componentCount; + } + } + + std::vector> res(componentCount); + + // Create the vertices. + for (auto const &vPair : _graph.Vertices()) + { + const auto &v = vPair.second.get(); + const auto &componentId = visited[v.Id()]; + res[componentId].AddVertex(v.Name(), v.Data(), v.Id()); + } + + // Create the edges. + for (auto const &ePair : _graph.Edges()) + { + const auto &e = ePair.second.get(); + const auto &vertices = e.Vertices(); + const auto &componentId = visited[vertices.first]; + res[componentId].AddEdge(vertices, e.Data(), e.Weight()); + } + + return res; + } + + /// \brief Copy a DirectedGraph to an UndirectedGraph with the same vertices + /// and edges. + /// \param[in] _graph A directed graph. + /// \return An undirected graph with the same vertices and edges as the + /// original graph. + template + UndirectedGraph ToUndirectedGraph(const DirectedGraph &_graph) + { + std::vector> vertices; + std::vector> edges; + + // Add all vertices. + for (auto const &vPair : _graph.Vertices()) + { + vertices.push_back(vPair.second.get()); + } + + // Add all edges. + for (auto const &ePair : _graph.Edges()) + { + auto const &e = ePair.second.get(); + edges.push_back({e.Vertices(), e.Data(), e.Weight()}); + } + + return UndirectedGraph(vertices, edges); + } +} +} +} +} +#endif diff --git a/include/gz/math/graph/Vertex.hh b/include/gz/math/graph/Vertex.hh new file mode 100644 index 000000000..46c8f007b --- /dev/null +++ b/include/gz/math/graph/Vertex.hh @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2017 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_GRAPH_VERTEX_HH_ +#define GZ_MATH_GRAPH_VERTEX_HH_ + +// uint64_t +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace ignition +{ +namespace math +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_MATH_VERSION_NAMESPACE { +namespace graph +{ + /// \typedef VertexId. + /// \brief The unique Id of each vertex. + using VertexId = uint64_t; + + /// \def VertexId_P + /// \brief A pair of two vertex Ids. + using VertexId_P = std::pair; + + /// \brief Represents an invalid Id. + static const VertexId kNullId = MAX_UI64; + + /// \brief A vertex of a graph. It stores user information, an optional name, + /// and keeps an internal unique Id. This class does not enforce to choose a + /// unique name. + template + class Vertex + { + /// \brief An invalid vertex. + public: static Vertex NullVertex; + + /// \brief Constructor. + /// \param[in] _name Non-unique vertex name. + /// \param[in] _data User information. + /// \param[in] _id Optional unique id. + public: Vertex(const std::string &_name, + const V &_data = V(), + const VertexId _id = kNullId) + : name(_name), + data(_data), + id(_id) + { + } + + /// \brief Retrieve the user information. + /// \return Reference to the user information. + public: const V &Data() const + { + return this->data; + } + + /// \brief Get a mutable reference to the user information. + /// \return Mutable reference to the user information. + public: V &Data() + { + return this->data; + } + + /// \brief Get the vertex Id. + /// \return The vertex Id. + public: VertexId Id() const + { + return this->id; + } + + /// \brief Get the vertex name. + /// \return The vertex name. + public: const std::string &Name() const + { + return this->name; + } + + /// \brief Set the vertex name. + /// \param[in] _name The vertex name. + public: void SetName(const std::string &_name) + { + this->name = _name; + } + + /// \brief Whether the vertex is considered valid or not (id==kNullId). + /// \return True when the vertex is valid or false otherwise (id==kNullId) + public: bool Valid() const + { + return this->id != kNullId; + } + + /// \brief Stream insertion operator. The output uses DOT graph + /// description language. + /// \param[out] _out The output stream. + /// \param[in] _v Vertex to write to the stream. + /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). + public: friend std::ostream &operator<<(std::ostream &_out, + const Vertex &_v) + { + _out << " " << _v.Id() << " [label=\"" << _v.Name() + << " (" << _v.Id() << ")\"];" << std::endl; + return _out; + } + + /// \brief Non-unique vertex name. + private: std::string name = ""; + + /// \brief User information. + private: V data; + + /// \brief Unique vertex Id. + private: VertexId id = kNullId; + }; + + /// \brief An invalid vertex. + template + Vertex Vertex::NullVertex("__null__", V(), kNullId); + + /// \def VertexRef_M + /// \brief Map of vertices. The key is the vertex Id. The value is a + /// reference to the vertex. + template + using VertexRef_M = + std::map>>; +} +} +} +} +#endif diff --git a/include/ignition/math/math.hh.in b/include/gz/math/math.hh.in similarity index 54% rename from include/ignition/math/math.hh.in rename to include/gz/math/math.hh.in index 4b76db86d..32d48f66a 100644 --- a/include/ignition/math/math.hh.in +++ b/include/gz/math/math.hh.in @@ -1,3 +1,3 @@ // Automatically generated -#include +#include ${ign_headers} diff --git a/include/ignition/math.hh b/include/ignition/math.hh new file mode 100644 index 000000000..cad1e2c96 --- /dev/null +++ b/include/ignition/math.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/math/AdditivelySeparableScalarField3.hh b/include/ignition/math/AdditivelySeparableScalarField3.hh index e05128a35..62620a374 100644 --- a/include/ignition/math/AdditivelySeparableScalarField3.hh +++ b/include/ignition/math/AdditivelySeparableScalarField3.hh @@ -13,185 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_SEPARABLE_SCALAR_FIELD3_HH_ -#define IGNITION_MATH_SEPARABLE_SCALAR_FIELD3_HH_ + */ -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /** \class AdditivelySeparableScalarField3\ - * AdditivelySeparableScalarField3.hh\ - * ignition/math/AdditivelySeparableScalarField3.hh - */ - /// \brief The AdditivelySeparableScalarField3 class constructs - /// a scalar field F in R^3 as a sum of scalar functions i.e. - /// F(x, y, z) = k (p(x) + q(y) + r(z)). - /// - /// \tparam ScalarFunctionT a callable type that taking a single ScalarT - /// value as argument and returning another ScalarT value. Additionally: - /// - for AdditivelySeparableScalarField3T to have a stream operator - /// overload, ScalarFunctionT must implement a - /// void Print(std::ostream &, const std::string &) method that streams - /// a representation of it using the given string as argument variable - /// name; - /// - for AdditivelySeparableScalarField3T::Minimum to be callable, - /// ScalarFunctionT must implement a - /// ScalarT Minimum(const Interval &, ScalarT &) method that - /// computes its minimum in the given interval and returns an argument - /// value that yields said minimum. - /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits - /// have been specialized. - /// - /// ## Example - /// - /// \snippet examples/additively_separable_scalar_field3_example.cc complete - template - class AdditivelySeparableScalarField3 - { - /// \brief Constructor - /// \param[in] _k scalar constant - /// \param[in] _p scalar function of x - /// \param[in] _q scalar function of y - /// \param[in] _r scalar function of z - public: AdditivelySeparableScalarField3( - ScalarT _k, ScalarFunctionT _p, - ScalarFunctionT _q, ScalarFunctionT _r) - : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r)) - { - } - - /// \brief Evaluate the scalar field at `_point` - /// \param[in] _point scalar field argument - /// \return the result of evaluating `F(_point)` - public: ScalarT Evaluate(const Vector3 &_point) const - { - return this->k * ( - this->p(_point.X()) + - this->q(_point.Y()) + - this->r(_point.Z())); - } - - /// \brief Call operator overload - /// \see SeparableScalarField3::Evaluate() - /// \param[in] _point scalar field argument - /// \return the result of evaluating `F(_point)` - public: ScalarT operator()(const Vector3 &_point) const - { - return this->Evaluate(_point); - } - - /// \brief Compute scalar field minimum in a `_region` - /// \param[in] _region scalar field argument set to check - /// \param[out] _pMin scalar field argument that yields - /// the minimum, or NaN if `_region` is empty - /// \return the scalar field minimum in the given `_region`, - /// or NaN if `_region` is empty - public: ScalarT Minimum(const Region3 &_region, - Vector3 &_pMin) const - { - if (_region.Empty()) - { - _pMin = Vector3::NaN; - return std::numeric_limits::quiet_NaN(); - } - return this->k * ( - this->p.Minimum(_region.Ix(), _pMin.X()) + - this->q.Minimum(_region.Iy(), _pMin.Y()) + - this->r.Minimum(_region.Iz(), _pMin.Z())); - } - - /// \brief Compute scalar field minimum in a `_region` - /// \param[in] _region scalar field argument set to check - /// \return the scalar field minimum in the given `_region`, - /// or NaN if `_region` is empty - public: ScalarT Minimum(const Region3 &_region) const - { - Vector3 pMin; - return this->Minimum(_region, pMin); - } - - /// \brief Compute scalar field minimum - /// \param[out] _pMin scalar field argument that yields - /// the minimum, or NaN if `_region` is empty - /// \return the scalar field minimum - public: ScalarT Minimum(Vector3 &_pMin) const - { - return this->Minimum(Region3::Unbounded, _pMin); - } - - /// \brief Compute scalar field minimum - /// \return the scalar field minimum - public: ScalarT Minimum() const - { - Vector3 pMin; - return this->Minimum(Region3::Unbounded, pMin); - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _field SeparableScalarField3 to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, - const ignition::math::AdditivelySeparableScalarField3< - ScalarFunctionT, ScalarT> &_field) - { - using std::abs; // enable ADL - constexpr ScalarT epsilon = - std::numeric_limits::epsilon(); - if ((abs(_field.k) - ScalarT(1)) < epsilon) - { - if (_field.k < ScalarT(0)) - { - _out << "-"; - } - } - else - { - _out << _field.k << " "; - } - _out << "[("; - _field.p.Print(_out, "x"); - _out << ") + ("; - _field.q.Print(_out, "y"); - _out << ") + ("; - _field.r.Print(_out, "z"); - return _out << ")]"; - } - - /// \brief Scalar constant - private: ScalarT k; - - /// \brief Scalar function of x - private: ScalarFunctionT p; - - /// \brief Scalar function of y - private: ScalarFunctionT q; - - /// \brief Scalar function of z - private: ScalarFunctionT r; - }; - - template - using AdditivelySeparableScalarField3f = - AdditivelySeparableScalarField3; - - template - using AdditivelySeparableScalarField3d = - AdditivelySeparableScalarField3; - } - } -} -#endif diff --git a/include/ignition/math/Angle.hh b/include/ignition/math/Angle.hh index 126fdbe5b..29188f1ec 100644 --- a/include/ignition/math/Angle.hh +++ b/include/ignition/math/Angle.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,251 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_ANGLE_HH_ -#define IGNITION_MATH_ANGLE_HH_ + */ -#include -#include +#include #include - -/// \def IGN_RTOD(d) -/// \brief Macro that converts radians to degrees -/// \param[in] r radians -/// \return degrees -#define IGN_RTOD(r) ((r) * 180 / IGN_PI) - -/// \def IGN_DTOR(d) -/// \brief Converts degrees to radians -/// \param[in] d degrees -/// \return radians -#define IGN_DTOR(d) ((d) * IGN_PI / 180) - -/// \def IGN_NORMALIZE(a) -/// \brief Macro that normalizes an angle in the range -Pi to Pi -/// \param[in] a angle -/// \return the angle, in range -#define IGN_NORMALIZE(a) (atan2(sin(a), cos(a))) - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Angle Angle.hh ignition/math/Angle.hh - /// \brief The Angle class is used to simplify and clarify the use of - /// radians and degrees measurements. A default constructed Angle instance - /// has a value of zero radians/degrees. - /// - /// Unless otherwise specified, the Angle class assumes units are in - /// radians. An example of this are the stream insertion (<<) and - /// extraction (>>) operators. - /// - /// ## Example - /// - /// \snippet examples/angle_example.cc complete - class IGNITION_MATH_VISIBLE Angle - { - /// \brief An angle with a value of zero. - /// Equivalent to math::Angle(0). - public: static const Angle Zero; - - /// \brief An angle with a value of Pi. - /// Equivalent to math::Angle(IGN_PI). - public: static const Angle Pi; - - /// \brief An angle with a value of Pi * 0.5. - /// Equivalent to math::Angle(IGN_PI * 0.5). - public: static const Angle HalfPi; - - /// \brief An angle with a value of Pi * 2. - /// Equivalent to math::Angle(IGN_PI * 2). - public: static const Angle TwoPi; - - /// \brief Default constructor that initializes an Angle to zero - /// radians/degrees. - public: Angle(); - - /// \brief Conversion constructor that initializes an Angle to the - /// specified radians. This constructor supports implicit conversion - /// of a double to an Angle. For example: - /// - /// \code - /// Angle a = 3.14; - /// \endcode - // - /// \param[in] _radian The radians used to initialize this Angle. - // cppcheck-suppress noExplicitConstructor - public: Angle(const double _radian); - - /// \brief Copy constructor that initializes this Angle to the value - /// contained in the _angle parameter. - /// \param[in] _angle Angle to copy - public: Angle(const Angle &_angle); - - /// \brief Move constructor - /// \param[in] _angle Angle to move - public: Angle(Angle &&_angle) noexcept; - - /// \brief Destructor - public: virtual ~Angle(); - - /// \brief Copy assignment operator - /// \param[in] _angle Angle to copy - public: Angle& operator=(const Angle &_angle); - - /// \brief Move assignment operator - /// \param[in] _angle Angle to move - public: Angle& operator=(Angle &&_angle) noexcept; - - /// \brief Set the value from an angle in radians. - /// \param[in] _radian Radian value. - /// \sa SetRadian(double) - public: void Radian(double _radian); - - /// \brief Set the value from an angle in radians. - /// \param[in] _radian Radian value. - public: void SetRadian(double _radian); - - /// \brief Set the value from an angle in degrees - /// \param[in] _degree Degree value - /// \sa SetDegree(double) - public: void Degree(double _degree); - - /// \brief Set the value from an angle in degrees - /// \param[in] _degree Degree value - public: void SetDegree(double _degree); - - /// \brief Get the angle in radians. - /// \return Double containing the angle's radian value. - public: double Radian() const; - - /// \brief Get the angle in degrees. - /// \return Double containing the angle's degree value. - public: double Degree() const; - - /// \brief Normalize the angle in the range -Pi to Pi. This - /// modifies the value contained in this Angle instance. - /// \sa Normalized() - public: void Normalize(); - - /// \brief Return the normalized angle in the range -Pi to Pi. This - /// does not modify the value contained in this Angle instance. - /// \return The normalized value of this Angle. - public: Angle Normalized() const; - - /// \brief Return the angle's radian value - /// \return double containing the angle's radian value - public: double operator()() const; - - /// \brief Dereference operator - /// \return Double containing the angle's radian value - public: inline double operator*() const - { - return value; - } - - /// \brief Subtraction operator, result = this - _angle. - /// \param[in] _angle Angle for subtraction. - /// \return The new angle. - public: Angle operator-(const Angle &_angle) const; - - /// \brief Addition operator, result = this + _angle. - /// \param[in] _angle Angle for addition. - /// \return The new angle. - public: Angle operator+(const Angle &_angle) const; - - /// \brief Multiplication operator, result = this * _angle. - /// \param[in] _angle Angle for multiplication. - /// \return The new angle - public: Angle operator*(const Angle &_angle) const; - - /// \brief Division operator, result = this / _angle. - /// \param[in] _angle Angle for division. - /// \return The new angle. - public: Angle operator/(const Angle &_angle) const; - - /// \brief Subtraction set operator, this = this - _angle. - /// \param[in] _angle Angle for subtraction. - /// \return The new angle. - public: Angle operator-=(const Angle &_angle); - - /// \brief Addition set operator, this = this + _angle. - /// \param[in] _angle Angle for addition. - /// \return The new angle. - public: Angle operator+=(const Angle &_angle); - - /// \brief Multiplication set operator, this = this * _angle. - /// \param[in] _angle Angle for multiplication. - /// \return The new angle. - public: Angle operator*=(const Angle &_angle); - - /// \brief Division set operator, this = this / _angle. - /// \param[in] _angle Angle for division. - /// \return The new angle. - public: Angle operator/=(const Angle &_angle); - - /// \brief Equality operator, result = this == _angle. - /// \param[in] _angle Angle to check for equality. - /// \return True if this == _angle. - public: bool operator==(const Angle &_angle) const; - - /// \brief Inequality operator - /// \param[in] _angle Angle to check for inequality. - /// \return True if this != _angle. - public: bool operator!=(const Angle &_angle) const; - - /// \brief Less than operator. - /// \param[in] _angle Angle to check. - /// \return True if this < _angle. - public: bool operator<(const Angle &_angle) const; - - /// \brief Less than or equal operator. - /// \param[in] _angle Angle to check. - /// \return True if this <= _angle. - public: bool operator<=(const Angle &_angle) const; - - /// \brief Greater than operator. - /// \param[in] _angle Angle to check. - /// \return True if this > _angle. - public: bool operator>(const Angle &_angle) const; - - /// \brief Greater than or equal operator. - /// \param[in] _angle Angle to check. - /// \return True if this >= _angle. - public: bool operator>=(const Angle &_angle) const; - - /// \brief Stream insertion operator. Outputs in radians. - /// \param[in] _out Output stream. - /// \param[in] _a Angle to output. - /// \return The output stream. - public: friend std::ostream &operator<<(std::ostream &_out, - const ignition::math::Angle &_a) - { - _out << _a.Radian(); - return _out; - } - - /// \brief Stream extraction operator. Assumes input is in radians. - /// \param[in,out] _in Input stream. - /// \param[out] _a Angle to read value into. - /// \return The input stream. - public: friend std::istream &operator>>(std::istream &_in, - ignition::math::Angle &_a) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - _in >> _a.value; - return _in; - } - - /// The angle in radians - private: double value{0}; - }; - } - } -} - -#endif diff --git a/include/ignition/math/AxisAlignedBox.hh b/include/ignition/math/AxisAlignedBox.hh index 6e50cac89..4229cbd72 100644 --- a/include/ignition/math/AxisAlignedBox.hh +++ b/include/ignition/math/AxisAlignedBox.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,303 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_AXISALIGNEDBOX_HH_ -#define IGNITION_MATH_AXISALIGNEDBOX_HH_ + */ -#include -#include +#include #include -#include -#include -#include -#include -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declaration of private data - class AxisAlignedBoxPrivate; - - /// \class AxisAlignedBox AxisAlignedBox.hh ignition/math/AxisAlignedBox.hh - /// \brief Mathematical representation of a box that is aligned along - /// an X,Y,Z axis. - class IGNITION_MATH_VISIBLE AxisAlignedBox - { - /// \brief Default constructor. This constructor will set the box's - /// minimum and maximum corners to the highest (max) and lowest - /// floating point values available to indicate that it is uninitialized. - /// The default box does not intersect any other boxes or contain any - /// points since it has no extent. Its center is the origin and its side - /// lengths are 0. - public: AxisAlignedBox(); - - /// \brief Constructor. This constructor will compute the box's - /// minimum and maximum corners based on the two arguments. - /// \param[in] _vec1 One corner of the box - /// \param[in] _vec2 Another corner of the box - public: AxisAlignedBox(const Vector3d &_vec1, const Vector3d &_vec2); - - /// \brief Constructor. This constructor will compute the box's - /// minimum and maximum corners based on the arguments. - /// \param[in] _vec1X One corner's X position - /// \param[in] _vec1Y One corner's Y position - /// \param[in] _vec1Z One corner's Z position - /// \param[in] _vec2X Other corner's X position - /// \param[in] _vec2Y Other corner's Y position - /// \param[in] _vec2Z Other corner's Z position - public: AxisAlignedBox(double _vec1X, double _vec1Y, double _vec1Z, - double _vec2X, double _vec2Y, double _vec2Z); - - /// \brief Copy Constructor - /// \param[in] _b AxisAlignedBox to copy - public: AxisAlignedBox(const AxisAlignedBox &_b); - - /// \brief Destructor - public: virtual ~AxisAlignedBox(); - - /// \brief Get the length along the x dimension - /// \return Double value of the length in the x dimension - public: double XLength() const; - - /// \brief Get the length along the y dimension - /// \return Double value of the length in the y dimension - public: double YLength() const; - - /// \brief Get the length along the z dimension - /// \return Double value of the length in the z dimension - public: double ZLength() const; - - /// \brief Get the size of the box - /// \return Size of the box - public: math::Vector3d Size() const; - - /// \brief Get the box center - /// \return The center position of the box - public: math::Vector3d Center() const; - - /// \brief Merge a box with this box - /// \param[in] _box AxisAlignedBox to add to this box - public: void Merge(const AxisAlignedBox &_box); - - /// \brief Assignment operator. Set this box to the parameter - /// \param[in] _b AxisAlignedBox to copy - /// \return The new box. - public: AxisAlignedBox &operator=(const AxisAlignedBox &_b); - - /// \brief Addition operator. result = this + _b - /// \param[in] _b AxisAlignedBox to add - /// \return The new box - public: AxisAlignedBox operator+(const AxisAlignedBox &_b) const; - - /// \brief Addition set operator. this = this + _b - /// \param[in] _b AxisAlignedBox to add - /// \return This new box - public: const AxisAlignedBox &operator+=(const AxisAlignedBox &_b); - - /// \brief Equality test operator - /// \param[in] _b AxisAlignedBox to test - /// \return True if equal - public: bool operator==(const AxisAlignedBox &_b) const; - - /// \brief Inequality test operator - /// \param[in] _b AxisAlignedBox to test - /// \return True if not equal - public: bool operator!=(const AxisAlignedBox &_b) const; - - /// \brief Subtract a vector from the min and max values - /// \param _v The vector to use during subtraction - /// \return The new box - public: AxisAlignedBox operator-(const Vector3d &_v); - - /// \brief Add a vector to the min and max values - /// \param _v The vector to use during addition - /// \return The new box - public: AxisAlignedBox operator+(const Vector3d &_v); - - /// \brief Subtract a vector from the min and max values - /// \param _v The vector to use during subtraction - /// \return The new box - public: AxisAlignedBox operator-(const Vector3d &_v) const; - - /// \brief Add a vector to the min and max values - /// \param _v The vector to use during addition - /// \return The new box - public: AxisAlignedBox operator+(const Vector3d &_v) const; - - /// \brief Output operator - /// \param[in] _out Output stream - /// \param[in] _b AxisAlignedBox to output to the stream - /// \return The stream - public: friend std::ostream &operator<<(std::ostream &_out, - const ignition::math::AxisAlignedBox &_b) - { - _out << "Min[" << _b.Min() << "] Max[" << _b.Max() << "]"; - return _out; - } - - /// \brief Get the minimum corner. - /// \return The Vector3d that is the minimum corner of the box. - public: const Vector3d &Min() const; - - /// \brief Get the maximum corner. - /// \return The Vector3d that is the maximum corner of the box. - public: const Vector3d &Max() const; - - /// \brief Get a mutable version of the minimum corner. - /// \return The Vector3d that is the minimum corner of the box. - public: Vector3d &Min(); - - /// \brief Get a mutable version of the maximum corner. - /// \return The Vector3d that is the maximum corner of the box. - public: Vector3d &Max(); - - /// \brief Test box intersection. This test will only work if - /// both box's minimum corner is less than or equal to their - /// maximum corner. - /// \param[in] _box AxisAlignedBox to check for intersection with - /// this box. - /// \return True if this box intersects _box. - public: bool Intersects(const AxisAlignedBox &_box) const; - - /// \brief Check if a point lies inside the box. - /// \param[in] _p Point to check. - /// \return True if the point is inside the box. - public: bool Contains(const Vector3d &_p) const; - - /// \brief Check if a ray (origin, direction) intersects the box. - /// \param[in] _origin Origin of the ray. - /// \param[in] _dir Direction of the ray. This ray will be normalized. - /// \param[in] _min Minimum allowed distance. - /// \param[in] _max Maximum allowed distance. - /// \return A boolean - public: bool IntersectCheck(const Vector3d &_origin, const Vector3d &_dir, - const double _min, const double _max) const; - - /// \brief Check if a ray (origin, direction) intersects the box. - /// \param[in] _origin Origin of the ray. - /// \param[in] _dir Direction of the ray. This ray will be normalized. - /// \param[in] _min Minimum allowed distance. - /// \param[in] _max Maximum allowed distance. - /// \return A boolean and double tuple. The boolean value is true - /// if the line intersects the box. - /// - /// The double is the distance from - /// the ray's start to the closest intersection point on the box, - /// minus the _min distance. For example, if _min == 0.5 and the - /// intersection happens at a distance of 2.0 from _origin then returned - /// distance is 1.5. - /// - /// The double value is zero when the boolean value is false. - public: std::tuple IntersectDist( - const Vector3d &_origin, const Vector3d &_dir, - const double _min, const double _max) const; - - /// \brief Check if a ray (origin, direction) intersects the box. - /// \param[in] _origin Origin of the ray. - /// \param[in] _dir Direction of the ray. This ray will be normalized. - /// \param[in] _min Minimum allowed distance. - /// \param[in] _max Maximum allowed distance. - /// \return A boolean, double, Vector3d tuple. The boolean value is true - /// if the line intersects the box. - /// - /// The double is the distance from the ray's start to the closest - /// intersection point on the box, - /// minus the _min distance. For example, if _min == 0.5 and the - /// intersection happens at a distance of 2.0 from _origin then returned - /// distance is 1.5. - /// The double value is zero when the boolean value is false. The - /// - /// Vector3d is the intersection point on the box. The Vector3d value - /// is zero if the boolean value is false. - public: std::tuple Intersect( - const Vector3d &_origin, const Vector3d &_dir, - const double _min, const double _max) const; - - /// \brief Check if a line intersects the box. - /// \param[in] _line The line to check against this box. - /// \return A boolean, double, Vector3d tuple. The boolean value is true - /// if the line intersects the box. The double is the distance from - /// the line's start to the closest intersection point on the box. - /// The double value is zero when the boolean value is false. The - /// Vector3d is the intersection point on the box. The Vector3d value - /// is zero if the boolean value is false. - public: std::tuple Intersect( - const Line3d &_line) const; - - /// \brief Get the volume of the box in m^3. - /// \return Volume of the box in m^3. - public: double Volume() const; - - /// \brief Compute the cylinder's density given a mass value. The - /// cylinder is assumed to be solid with uniform density. This - /// function requires the cylinder's radius and length to be set to - /// values greater than zero. The Material of the cylinder is ignored. - /// \param[in] _mass Mass of the cylinder, in kg. This value should be - /// greater than zero. - /// \return Density of the cylinder in kg/m^3. A negative value is - /// returned if radius, length or _mass is <= 0. - /// \deprecated Unimplemented - public: double IGN_DEPRECATED(6.0) DensityFromMass( - const double _mass) const; - - /// \brief Set the density of this box based on a mass value. - /// Density is computed using - /// double DensityFromMass(const double _mass) const. The - /// box is assumed to be solid with uniform density. This - /// function requires the box's size to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the box, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// box's size or the _mass value are <= 0. - /// \sa double DensityFromMass(const double _mass) const - /// \deprecated Unimplemented - public: bool IGN_DEPRECATED(6.0) SetDensityFromMass( - const double _mass); - - /// \brief Get the material associated with this box. - /// \return The material assigned to this box. - /// \deprecated Unimplemented - public: const ignition::math::Material IGN_DEPRECATED(6.0) - &Material() const; - - /// \brief Set the material associated with this box. - /// \param[in] _mat The material assigned to this box - /// \deprecated Unimplemented - public: void IGN_DEPRECATED(6.0) SetMaterial( - const ignition::math::Material &_mat); - - /// \brief Get the mass matrix for this box. This function - /// is only meaningful if the box's size and material - /// have been set. - /// \param[out] _massMat The computed mass matrix will be stored - /// here. - /// \return False if computation of the mass matrix failed, which - /// could be due to an invalid size (<=0) or density (<=0). - /// \deprecated Unimplemented - public: bool IGN_DEPRECATED(6.0) MassMatrix( - MassMatrix3d &_massMat) const; - - /// \brief Clip a line to a dimension of the box. - /// This is a helper function to Intersects - /// \param[in] _d Dimension of the box(0, 1, or 2). - /// \param[in] _line Line to clip - /// \param[in,out] _low Close distance - /// \param[in,out] _high Far distance - private: bool ClipLine(const int _d, const Line3d &_line, - double &_low, double &_high) const; - - /// \brief Private data pointer - private: AxisAlignedBoxPrivate *dataPtr; - }; - } - } -} -#endif diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 09a55ec59..f517d2dea 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,213 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_BOX_HH_ -#define IGNITION_MATH_BOX_HH_ + */ +#include #include -#include -#include -#include -#include - -#include "ignition/math/detail/WellOrderedVector.hh" - -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - /// \brief This is the type used for deduplicating and returning the set of - /// intersections. - template - using IntersectionPoints = std::set, WellOrderedVectors>; - - /// \class Box Box.hh ignition/math/Box.hh - /// \brief A representation of a box. All units are in meters. - /// - /// The box class supports defining a size and material properties. - /// See Material for more on material properties. - /// - /// By default, a box's size (length, width, and height) is zero. - /// - /// See AxisAlignedBox for an axis aligned box implementation. - template - class Box - { - /// \brief Default constructor. - public: Box() = default; - - /// \brief Construct a box with specified dimensions. - /// \param[in] _length Length of the box in meters. - /// \param[in] _width Width of the box in meters. - /// \param[in] _height Height of the box in meters. - public: Box(const Precision _length, - const Precision _width, - const Precision _height); - - /// \brief Construct a box with specified dimensions and a material. - /// \param[in] _length Length of the box in meters. - /// \param[in] _width Width of the box in meters. - /// \param[in] _height Height of the box. - /// \param[in] _mat Material property for the box. - public: Box(const Precision _length, const Precision _width, - const Precision _height, - const ignition::math::Material &_mat); - - /// \brief Construct a box with specified dimensions, in vector form. - /// \param[in] _size Size of the box. The vector _size has the following - /// mapping: - /// - /// * _size[0] == length in meters - /// * _size[1] == width in meters - /// * _size[2] == height in meters - public: explicit Box(const Vector3 &_size); - - /// \brief Construct a box with specified dimensions, in vector form - /// and a material. - /// \param[in] _size Size of the box. The vector _size has the following - /// mapping: - /// - /// * _size[0] == length in meters - /// * _size[1] == width in meters - /// * _size[2] == height in meters - /// \param[in] _mat Material property for the box. - public: Box(const Vector3 &_size, - const ignition::math::Material &_mat); - - /// \brief Destructor. - public: virtual ~Box() = default; - - /// \brief Get the size of the box. - /// \return Size of the box in meters. - public: math::Vector3 Size() const; - - /// \brief Set the size of the box. - /// \param[in] _size Size of the box. The vector _size has the following - /// mapping: - /// - /// * _size[0] == lengt in metersh - /// * _size[1] == widt in metersh - /// * _size[2] == heigh in meterst - public: void SetSize(const math::Vector3 &_size); - - /// \brief Set the size of the box. - /// \param[in] _length Length of the box in meters. - /// \param[in] _width Width of the box in meters. - /// \param[in] _height Height of the box in meters. - public: void SetSize(const Precision _length, - const Precision _width, - const Precision _height); - - /// \brief Equality test operator. - /// \param[in] _b Box to test. - /// \return True if equal. - public: bool operator==(const Box &_b) const; - - /// \brief Inequality test operator. - /// \param[in] _b Box to test. - /// \return True if not equal. - public: bool operator!=(const Box &_b) const; - - /// \brief Get the material associated with this box. - /// \return The material assigned to this box. - public: const ignition::math::Material &Material() const; - - /// \brief Set the material associated with this box. - /// \param[in] _mat The material assigned to this box. - public: void SetMaterial(const ignition::math::Material &_mat); - - /// \brief Get the volume of the box in m^3. - /// \return Volume of the box in m^3. - public: Precision Volume() const; - - /// \brief Get the volume of the box below a plane. - /// \param[in] _plane The plane which cuts the box, expressed in the box's - /// frame. - /// \return Volume below the plane in m^3. - public: Precision VolumeBelow(const Plane &_plane) const; - - /// \brief Center of volume below the plane. This is useful when - /// calculating where buoyancy should be applied, for example. - /// \param[in] _plane The plane which cuts the box, expressed in the box's - /// frame. - /// \return Center of volume, in box's frame. - public: std::optional> - CenterOfVolumeBelow(const Plane &_plane) const; - - /// \brief All the vertices which are on or below the plane. - /// \param[in] _plane The plane which cuts the box, expressed in the box's - /// frame. - /// \return Box vertices which are below the plane, expressed in the box's - /// frame. - public: IntersectionPoints - VerticesBelow(const Plane &_plane) const; - - /// \brief Compute the box's density given a mass value. The - /// box is assumed to be solid with uniform density. This - /// function requires the box's size to be set to - /// values greater than zero. The Material of the box is ignored. - /// \param[in] _mass Mass of the box, in kg. This value should be - /// greater than zero. - /// \return Density of the box in kg/m^3. A negative value is - /// returned if the size or _mass is <= 0. - public: Precision DensityFromMass(const Precision _mass) const; - - /// \brief Set the density of this box based on a mass value. - /// Density is computed using - /// double DensityFromMass(const double _mass) const. The - /// box is assumed to be solid with uniform density. This - /// function requires the box's size to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the box, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// box's size or the _mass value are <= 0. - /// \sa double DensityFromMass(const double _mass) const - public: bool SetDensityFromMass(const Precision _mass); - - /// \brief Get the mass matrix for this box. This function - /// is only meaningful if the box's size and material - /// have been set. - /// \param[out] _massMat The computed mass matrix will be stored - /// here. - /// \return False if computation of the mass matrix failed, which - /// could be due to an invalid size (<=0) or density (<=0). - public: bool MassMatrix(MassMatrix3 &_massMat) const; - - /// \brief Get intersection between a plane and the box's edges. - /// Edges contained on the plane are ignored. - /// \param[in] _plane The plane against which we are testing intersection. - /// \returns A list of points along the edges of the box where the - /// intersection occurs. - public: IntersectionPoints Intersections( - const Plane &_plane) const; - - /// \brief Size of the box. - private: Vector3 size = Vector3::Zero; - - /// \brief The box's material. - private: ignition::math::Material material; - }; - - /// \typedef Box Boxi - /// \brief Box with integer precision. - typedef Box Boxi; - - /// \typedef Box Boxd - /// \brief Box with double precision. - typedef Box Boxd; - - /// \typedef Box Boxf - /// \brief Box with float precision. - typedef Box Boxf; - } - } -} -#include "ignition/math/detail/Box.hh" -#endif diff --git a/include/ignition/math/Capsule.hh b/include/ignition/math/Capsule.hh index cbf4ae198..cd30f01c4 100644 --- a/include/ignition/math/Capsule.hh +++ b/include/ignition/math/Capsule.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. @@ -13,139 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_CAPSULE_HH_ -#define IGNITION_MATH_CAPSULE_HH_ + */ -#include -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" - -namespace ignition -{ - namespace math - { - // Foward declarations - class CapsulePrivate; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Capsule Capsule.hh ignition/math/Capsule.hh - /// \brief A representation of a capsule or sphere-capped cylinder. - /// - /// The capsule class supports defining a capsule with a radius, - /// length, and material properties. The shape is equivalent to a cylinder - /// aligned with the Z-axis and capped with hemispheres. Radius and - /// length are in meters. See Material for more on material properties. - /// \tparam Precision Scalar numeric type. - template - class Capsule - { - /// \brief Default constructor. The default radius and length are both - /// zero. - public: Capsule() = default; - - /// \brief Construct a capsule with a length and radius. - /// \param[in] _length Length of the capsule. - /// \param[in] _radius Radius of the capsule. - public: Capsule(const Precision _length, const Precision _radius); - - /// \brief Construct a capsule with a length, radius, and material. - /// \param[in] _length Length of the capsule. - /// \param[in] _radius Radius of the capsule. - /// \param[in] _mat Material property for the capsule. - public: Capsule(const Precision _length, const Precision _radius, - const Material &_mat); - - /// \brief Get the radius in meters. - /// \return The radius of the capsule in meters. - public: Precision Radius() const; - - /// \brief Set the radius in meters. - /// \param[in] _radius The radius of the capsule in meters. - public: void SetRadius(const Precision _radius); - - /// \brief Get the length in meters. - /// \return The length of the capsule in meters. - public: Precision Length() const; - - /// \brief Set the length in meters. - /// \param[in] _length The length of the capsule in meters. - public: void SetLength(const Precision _length); - - /// \brief Get the material associated with this capsule. - /// \return The material assigned to this capsule - public: const Material &Mat() const; - - /// \brief Set the material associated with this capsule. - /// \param[in] _mat The material assigned to this capsule - public: void SetMat(const Material &_mat); - - /// \brief Get the mass matrix for this capsule. This function - /// is only meaningful if the capsule's radius, length, and material - /// have been set. - /// \return The computed mass matrix if parameters are valid - /// (radius > 0), (length > 0), and (density > 0). Otherwise - /// std::nullopt is returned. - public: std::optional< MassMatrix3 > MassMatrix() const; - - /// \brief Check if this capsule is equal to the provided capsule. - /// Radius, length, and material properties will be checked. - public: bool operator==(const Capsule &_capsule) const; - - /// \brief Get the volume of the capsule in m^3. - /// \return Volume of the capsule in m^3. - public: Precision Volume() const; - - /// \brief Compute the capsule's density given a mass value. The - /// capsule is assumed to be solid with uniform density. This - /// function requires the capsule's radius and length to be set to - /// values greater than zero. The Material of the capsule is ignored. - /// \param[in] _mass Mass of the capsule, in kg. This value should be - /// greater than zero. - /// \return Density of the capsule in kg/m^3. A NaN is returned - /// if radius, length or _mass is <= 0. - public: Precision DensityFromMass(const Precision _mass) const; - - /// \brief Set the density of this capsule based on a mass value. - /// Density is computed using - /// Precision DensityFromMass(const Precision _mass) const. The - /// capsule is assumed to be solid with uniform density. This - /// function requires the capsule's radius and length to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the capsule, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// capsule's radius, length, or the _mass value are <= 0. - /// \sa Precision DensityFromMass(const Precision _mass) const - public: bool SetDensityFromMass(const Precision _mass); - - /// \brief Radius of the capsule. - private: Precision radius = 0.0; - - /// \brief Length of the capsule. - private: Precision length = 0.0; - - /// \brief the capsule's material. - private: Material material; - }; - - /// \typedef Capsule Capsulei - /// \brief Capsule with integer precision. - typedef Capsule Capsulei; - - /// \typedef Capsule Capsuled - /// \brief Capsule with double precision. - typedef Capsule Capsuled; - - /// \typedef Capsule Capsulef - /// \brief Capsule with float precision. - typedef Capsule Capsulef; - } - } -} -#include "ignition/math/detail/Capsule.hh" - -#endif +#include +#include diff --git a/include/ignition/math/Color.hh b/include/ignition/math/Color.hh index fd6592941..4d25c3c78 100644 --- a/include/ignition/math/Color.hh +++ b/include/ignition/math/Color.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,350 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_COLOR_HH_ -#define IGNITION_MATH_COLOR_HH_ + */ -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Color Color.hh ignition/math/Color.hh - /// \brief Defines a color using a red (R), green (G), blue (B), and alpha - /// (A) component. Each color component is in the range [0..1]. - /// - /// ## Example - /// - /// \snippet examples/color_example.cc complete - class IGNITION_MATH_VISIBLE Color - { - /// \brief (1, 1, 1) - public: static const Color White; - /// \brief (0, 0, 0) - public: static const Color Black; - /// \brief (1, 0, 0) - public: static const Color Red; - /// \brief (0, 1, 0) - public: static const Color Green; - /// \brief (0, 0, 1) - public: static const Color Blue; - /// \brief (1, 1, 0) - public: static const Color Yellow; - /// \brief (1, 0, 1) - public: static const Color Magenta; - /// \brief (0, 1, 1) - public: static const Color Cyan; - - /// \typedef RGBA - /// \brief A RGBA packed value as an unsigned int - /// Each 8 bits corresponds to a channel. - /// - /// \code - /// RGBA a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red. - /// \endcode - public: typedef unsigned int RGBA; - - /// \typedef BGRA - /// \brief A BGRA packed value as an unsigned int - /// Each 8 bits corresponds to a channel. - /// - /// \code - /// BGRA a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue. - /// \endcode - public: typedef unsigned int BGRA; - - /// \typedef ARGB - /// \brief A ARGB packed value as an unsigned int - /// Each 8 bits corresponds to a channel. - /// - /// \code - /// ARGB a = 0xFF0000FF; // (0, 0, 1, 1) for RGBA, i.e. blue. - /// \endcode - public: typedef unsigned int ARGB; - - /// \typedef ABGR - /// \brief A ABGR packed value as an unsigned int - /// Each 8 bits corresponds to a channel. - /// - /// \code - /// ABGR a = 0xFF0000FF; // (1, 0, 0, 1) for RGBA, i.e. red. - /// \endcode - public: typedef unsigned int ABGR; - - /// \brief Constructor - public: Color(); - - /// \brief Constructor - /// \param[in] _r Red value (range 0 to 1) - /// \param[in] _g Green value (range 0 to 1 - /// \param[in] _b Blue value (range 0 to 1 - /// \param[in] _a Alpha value (0=transparent, 1=opaque) - public: Color(const float _r, const float _g, const float _b, - const float _a = 1.0); - - /// \brief Copy Constructor - /// \param[in] _clr Color to copy - public: Color(const Color &_clr); - - /// \brief Destructor - public: virtual ~Color(); - - /// \brief Reset the color to default values to red=0, green=0, - /// blue=0, alpha=1. - public: void Reset(); - - /// \brief Set the contents of the vector - /// \param[in] _r Red value (range 0 to 1) - /// \param[in] _g Green value (range 0 to 1) - /// \param[in] _b Blue value (range 0 to 1) - /// \param[in] _a Alpha value (0=transparent, 1=opaque) - public: void Set(const float _r = 1, const float _g = 1, - const float _b = 1, const float _a = 1); - - /// \brief Get the color in HSV colorspace - /// \return HSV values in a Vector3f format. A vector3f containing - /// {NAN_F, NAN_F, NAN_F} is returned on error. - public: Vector3f HSV() const; - - /// \brief Set a color based on HSV values - /// \param[in] _h Hue(0..360) - /// \param[in] _s Saturation(0..1) - /// \param[in] _v Value(0..1) - public: void SetFromHSV(const float _h, const float _s, const float _v); - - /// \brief Get the color in YUV colorspace - /// \return the YUV color - public: Vector3f YUV() const; - - /// \brief Set from yuv - /// \param[in] _y value - /// \param[in] _u value - /// \param[in] _v value - public: void SetFromYUV(const float _y, const float _u, const float _v); - - /// \brief Equal operator - /// \param[in] _pt Color to copy - /// \return Reference to this color - public: Color &operator=(const Color &_pt); - - /// \brief Array index operator - /// \param[in] _index Color component index(0=red, 1=green, 2=blue) - /// \return r, g, b, or a when _index is 0, 1, 2 or 3. A NAN_F value is - /// returned if the _index is invalid - public: float operator[](const unsigned int _index); - - /// \brief Get as uint32 RGBA packed value - /// \return the color - public: RGBA AsRGBA() const; - - /// \brief Get as uint32 BGRA packed value - /// \return the color - public: BGRA AsBGRA() const; - - /// \brief Get as uint32 ARGB packed value - /// \return the color - public: ARGB AsARGB() const; - - /// \brief Get as uint32 ABGR packed value - /// \return the color - public: ABGR AsABGR() const; - - /// \brief Set from uint32 RGBA packed value - /// \param[in] _v the new color - public: void SetFromRGBA(const RGBA _v); - - /// \brief Set from uint32 BGRA packed value - /// \param[in] _v the new color - public: void SetFromBGRA(const BGRA _v); - - /// \brief Set from uint32 ARGB packed value - /// \param[in] _v the new color - public: void SetFromARGB(const ARGB _v); - - /// \brief Set from uint32 ABGR packed value - /// \param[in] _v the new color - public: void SetFromABGR(const ABGR _v); - - /// \brief Addition operator (this + _pt) - /// \param[in] _pt Color to add - /// \return The resulting color - public: Color operator+(const Color &_pt) const; - - /// \brief Add _v to all color components - /// \param[in] _v Value to add to each color component - /// \return The resulting color - public: Color operator+(const float &_v) const; - - /// \brief Addition equal operator - /// \param[in] _pt Color to add - /// \return The resulting color - public: const Color &operator+=(const Color &_pt); - - /// \brief Subtraction operator - /// \param[in] _pt The color to substract - /// \return The resulting color - public: Color operator-(const Color &_pt) const; - - /// \brief Subtract _v from all color components - /// \param[in] _v Value to subtract - /// \return The resulting color - public: Color operator-(const float &_v) const; - - /// \brief Subtraction equal operator - /// \param[in] _pt Color to subtract - /// \return The resulting color - public: const Color &operator-=(const Color &_pt); - - /// \brief Division operator - /// \param[in] _pt Color to divide by - /// \return The resulting color - public: const Color operator/(const Color &_pt) const; - - /// \brief Divide all color component by _v - /// \param[in] _v The value to divide by - /// \return The resulting color - public: const Color operator/(const float &_v) const; - - /// \brief Division equal operator - /// \param[in] _pt Color to divide by - /// \return The resulting color - public: const Color &operator/=(const Color &_pt); - - /// \brief Multiplication operator - /// \param[in] _pt The color to muliply by - /// \return The resulting color - public: const Color operator*(const Color &_pt) const; - - /// \brief Multiply all color components by _v - /// \param[in] _v The value to multiply by - /// \return The resulting color - public: const Color operator*(const float &_v) const; - - /// \brief Multiplication equal operator - /// \param[in] _pt The color to muliply by - /// \return The resulting color - public: const Color &operator*=(const Color &_pt); - - /// \brief Equality operator - /// \param[in] _pt The color to check for equality - /// \return True if the this color equals _pt - public: bool operator==(const Color &_pt) const; - - /// \brief Inequality operator - /// \param[in] _pt The color to check for inequality - /// \return True if the this color does not equal _pt - public: bool operator!=(const Color &_pt) const; - - /// \brief Clamp the color values to valid ranges - private: void Clamp(); - - /// \brief Stream insertion operator - /// \param[in] _out the output stream - /// \param[in] _pt the color - /// \return the output stream - public: friend std::ostream &operator<<(std::ostream &_out, - const Color &_pt) - { - _out << _pt.r << " " << _pt.g << " " << _pt.b << " " << _pt.a; - return _out; - } - - /// \brief Stream insertion operator - /// \param[in] _in the input stream. If the input stream does not include - /// an alpha value, a default alpha value of 1.0 will be used. - /// \param[in] _pt - public: friend std::istream &operator>> (std::istream &_in, Color &_pt) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - _in >> _pt.r >> _pt.g >> _pt.b; - // Since alpha is optional, check if it's there before parsing - while (_in.good() && std::isspace(_in.peek())) - { - _in.get(); - } - if (_in.good()) - { - _in >> _pt.a; - } - else if (!_in.fail()) - { - _pt.a = 1.0; - } - return _in; - } - - /// \brief Get the red value - /// \return The red value - public: float R() const; - - /// \brief Get the green value - /// \return The green value - public: float G() const; - - /// \brief Get the blue value - /// \return The blue value - public: float B() const; - - /// \brief Get the alpha value - /// \return The alpha value - public: float A() const; - - /// \brief Get a mutable reference to the red value - /// \return The red value - public: float &R(); - - /// \brief Get a mutable reference to the green value - /// \return The green value - public: float &G(); - - /// \brief Get a mutable reference to the blue value - /// \return The blue value - public: float &B(); - - /// \brief Get a mutable reference to the alpha value - /// \return The alpha value - public: float &A(); - - /// \brief Set the red value - /// \param[in] _r New red value - public: void R(const float _r); - - /// \brief Set the green value - /// \param[in] _g New green value - public: void G(const float _g); - - /// \brief Set the blue value - /// \param[in] _b New blue value - public: void B(const float _b); - - /// \brief Set the alpha value - /// \param[in] _a New alpha value - public: void A(const float _a); - - /// \brief Red value - private: float r = 0; - - /// \brief Green value - private: float g = 0; - - /// \brief Blue value - private: float b = 0; - - /// \brief Alpha value - private: float a = 1; - }; - } - } -} -#endif diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 161e5a48e..9742d05ad 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,171 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_CYLINDER_HH_ -#define IGNITION_MATH_CYLINDER_HH_ + */ -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" -#include "ignition/math/Quaternion.hh" - -namespace ignition -{ - namespace math - { - // Foward declarations - class CylinderPrivate; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Cylinder Cylinder.hh ignition/math/Cylinder.hh - /// \brief A representation of a cylinder. - /// - /// The cylinder class supports defining a cylinder with a radius, - /// length, rotational offset, and material properties. Radius and - /// length are in meters. See Material for more on material properties. - /// By default, a cylinder's length is aligned with the Z axis. The - /// rotational offset encodes a rotation from the z axis. - template - class Cylinder - { - /// \brief Default constructor. The default radius and length are both - /// zero. The default rotational offset is - /// Quaternion::Identity. - public: Cylinder() = default; - - /// \brief Construct a cylinder with a length, radius, and optionally - /// a rotational offset. - /// \param[in] _length Length of the cylinder. - /// \param[in] _radius Radius of the cylinder. - /// \param[in] _rotOffset Rotational offset of the cylinder. - public: Cylinder(const Precision _length, const Precision _radius, - const Quaternion &_rotOffset = - Quaternion::Identity); - - /// \brief Construct a cylinder with a length, radius, material and - /// optionally a rotational offset. - /// \param[in] _length Length of the cylinder. - /// \param[in] _radius Radius of the cylinder. - /// \param[in] _mat Material property for the cylinder. - /// \param[in] _rotOffset Rotational offset of the cylinder. - public: Cylinder(const Precision _length, const Precision _radius, - const Material &_mat, - const Quaternion &_rotOffset = - Quaternion::Identity); - - /// \brief Destructor - public: ~Cylinder() = default; - - /// \brief Get the radius in meters. - /// \return The radius of the cylinder in meters. - public: Precision Radius() const; - - /// \brief Set the radius in meters. - /// \param[in] _radius The radius of the cylinder in meters. - public: void SetRadius(const Precision _radius); - - /// \brief Get the length in meters. - /// \return The length of the cylinder in meters. - public: Precision Length() const; - - /// \brief Set the length in meters. - /// \param[in] _length The length of the cylinder in meters. - public: void SetLength(const Precision _length); - - /// \brief Get the rotational offset. By default, a cylinder's length - /// is aligned with the Z axis. The rotational offset encodes - /// a rotation from the z axis. - /// \return The cylinder's rotational offset. - /// \sa void SetRotationalOffset(const Quaternion &_rot) - public: Quaternion RotationalOffset() const; - - /// \brief Set the rotation offset. - /// See Quaternion RotationalOffset() for details on the - /// rotational offset. - /// \sa Quaternion RotationalOffset() const - public: void SetRotationalOffset( - const Quaternion &_rotOffset); - - /// \brief Get the material associated with this cylinder. - /// \return The material assigned to this cylinder - public: const Material &Mat() const; - - /// \brief Set the material associated with this cylinder. - /// \param[in] _mat The material assigned to this cylinder - public: void SetMat(const Material &_mat); - - /// \brief Get the mass matrix for this cylinder. This function - /// is only meaningful if the cylinder's radius, length, and material - /// have been set. Optionally, set the rotational offset. - /// \param[out] _massMat The computed mass matrix will be stored - /// here. - /// \return False if computation of the mass matrix failed, which - /// could be due to an invalid radius (<=0), length (<=0), or density - /// (<=0). - public: bool MassMatrix(MassMatrix3d &_massMat) const; - - /// \brief Check if this cylinder is equal to the provided cylinder. - /// Radius, length, and material properties will be checked. - public: bool operator==(const Cylinder &_cylinder) const; - - /// \brief Get the volume of the cylinder in m^3. - /// \return Volume of the cylinder in m^3. - public: Precision Volume() const; - - /// \brief Compute the cylinder's density given a mass value. The - /// cylinder is assumed to be solid with uniform density. This - /// function requires the cylinder's radius and length to be set to - /// values greater than zero. The Material of the cylinder is ignored. - /// \param[in] _mass Mass of the cylinder, in kg. This value should be - /// greater than zero. - /// \return Density of the cylinder in kg/m^3. A negative value is - /// returned if radius, length or _mass is <= 0. - public: Precision DensityFromMass(const Precision _mass) const; - - /// \brief Set the density of this cylinder based on a mass value. - /// Density is computed using - /// Precision DensityFromMass(const Precision _mass) const. The - /// cylinder is assumed to be solid with uniform density. This - /// function requires the cylinder's radius and length to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the cylinder, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// cylinder's radius, length, or the _mass value are <= 0. - /// \sa Precision DensityFromMass(const Precision _mass) const - public: bool SetDensityFromMass(const Precision _mass); - - /// \brief Radius of the cylinder. - private: Precision radius = 0.0; - - /// \brief Length of the cylinder. - private: Precision length = 0.0; - - /// \brief the cylinder's material. - private: Material material; - - /// \brief Rotational offset. - private: Quaternion rotOffset = - Quaternion::Identity; - }; - - /// \typedef Cylinder Cylinderi - /// \brief Cylinder with integer precision. - typedef Cylinder Cylinderi; - - /// \typedef Cylinder Cylinderd - /// \brief Cylinder with double precision. - typedef Cylinder Cylinderd; - - /// \typedef Cylinder Cylinderf - /// \brief Cylinder with float precision. - typedef Cylinder Cylinderf; - } - } -} -#include "ignition/math/detail/Cylinder.hh" - -#endif +#include +#include diff --git a/include/ignition/math/DiffDriveOdometry.hh b/include/ignition/math/DiffDriveOdometry.hh index 93d272155..360400b56 100644 --- a/include/ignition/math/DiffDriveOdometry.hh +++ b/include/ignition/math/DiffDriveOdometry.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,143 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DIFFDRIVEODOMETRY_HH_ -#define IGNITION_MATH_DIFFDRIVEODOMETRY_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Use a steady clock - using clock = std::chrono::steady_clock; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declarations. - class DiffDriveOdometryPrivate; - - /** \class DiffDriveOdometry DiffDriveOdometry.hh \ - * ignition/math/DiffDriveOdometry.hh - **/ - /// \brief Computes odometry values based on a set of kinematic - /// properties and wheel speeds for a diff-drive vehicle. - /// - /// A vehicle with a heading of zero degrees has a local - /// reference frame according to the diagram below. - /// - /// Y - /// ^ - /// | - /// | - /// O--->X(forward) - /// - /// Rotating the right wheel while keeping the left wheel fixed will cause - /// the vehicle to rotate counter-clockwise. For example (excuse the - /// lack of precision with ASCII art): - /// - /// Y X(forward) - /// ^ ^ - /// \ / - /// \ / - /// O - /// - /// **Example Usage** - /// - /// \code{.cpp} - /// ignition::math::DiffDriveOdometry odom; - /// odom.SetWheelParams(2.0, 0.5, 0.5); - /// odom.Init(std::chrono::steady_clock::now()); - /// - /// // ... Some time later - /// - /// // Both wheels have rotated the same amount - /// odom.Update(IGN_DTOR(2), IGN_DTOR(2), std::chrono::steady_clock::now()); - /// - /// // ... Some time later - /// - /// // The left wheel has rotated, the right wheel did not rotate - /// odom.Update(IGN_DTOR(4), IGN_DTOR(2), std::chrono::steady_clock::now()); - /// \endcode - class IGNITION_MATH_VISIBLE DiffDriveOdometry - { - /// \brief Constructor. - /// \param[in] _windowSize Rolling window size used to compute the - /// velocity mean - public: explicit DiffDriveOdometry(size_t _windowSize = 10); - - /// \brief Destructor. - public: ~DiffDriveOdometry(); - - /// \brief Initialize the odometry - /// \param[in] _time Current time. - public: void Init(const clock::time_point &_time); - - /// \brief Get whether Init has been called. - /// \return True if Init has been called, false otherwise. - public: bool Initialized() const; - - /// \brief Updates the odometry class with latest wheels and - /// steerings position - /// \param[in] _leftPos Left wheel position in radians. - /// \param[in] _rightPos Right wheel postion in radians. - /// \param[in] _time Current time point. - /// \return True if the odometry is actually updated. - public: bool Update(const Angle &_leftPos, const Angle &_rightPos, - const clock::time_point &_time); - - /// \brief Get the heading. - /// \return The heading in radians. - public: const Angle &Heading() const; - - /// \brief Get the X position. - /// \return The X position in meters - public: double X() const; - - /// \brief Get the Y position. - /// \return The Y position in meters. - public: double Y() const; - - /// \brief Get the linear velocity. - /// \return The linear velocity in meter/second. - public: double LinearVelocity() const; - - /// \brief Get the angular velocity. - /// \return The angular velocity in radian/second. - public: const Angle &AngularVelocity() const; - - /// \brief Set the wheel parameters including the radius and separation. - /// \param[in] _wheelSeparation Distance between left and right wheels. - /// \param[in] _leftWheelRadius Radius of the left wheel. - /// \param[in] _rightWheelRadius Radius of the right wheel. - public: void SetWheelParams(double _wheelSeparation, - double _leftWheelRadius, double _rightWheelRadius); - - /// \brief Set the velocity rolling window size. - /// \param[in] _size The Velocity rolling window size. - public: void SetVelocityRollingWindowSize(size_t _size); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} - -#endif diff --git a/include/ignition/math/Ellipsoid.hh b/include/ignition/math/Ellipsoid.hh index 4d67fa9ba..f836ca057 100644 --- a/include/ignition/math/Ellipsoid.hh +++ b/include/ignition/math/Ellipsoid.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. @@ -13,122 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_ELLIPSOID_HH_ -#define IGNITION_MATH_ELLIPSOID_HH_ + */ -#include -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Ellipsoid Ellipsoid.hh ignition/math/Ellipsoid.hh - /// \brief A representation of a general ellipsoid. - /// - /// The ellipsoid class supports defining a ellipsoid with three radii and - /// material properties. Radii are in meters. See Material for more on - /// material properties. - /// \tparam Precision Scalar numeric type. - template - class Ellipsoid - { - /// \brief Default constructor. The default radius and length are both - /// zero. - public: Ellipsoid() = default; - - /// \brief Construct a ellipsoid with a Vector3 of three radii. - /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid - public: explicit Ellipsoid(const Vector3 &_radii); - - /// \brief Construct a ellipsoid with three radii and a material. - /// \param[in] _radii The three radii (x, y, z) defining this ellipsoid - /// \param[in] _mat Material property for the ellipsoid. - public: Ellipsoid(const Vector3 &_radii, - const Material &_mat); - - /// \brief Get the radius in meters. - /// \return The radius of the ellipsoid in meters. - public: Vector3 Radii() const; - - /// \brief Set the radius in meters. - /// \param[in] _radii The radii of the ellipsoid in meters. - public: void SetRadii(const Vector3 &_radii); - - /// \brief Get the material associated with this ellipsoid. - /// \return The material assigned to this ellipsoid - public: const Material &Mat() const; - - /// \brief Set the material associated with this ellipsoid. - /// \param[in] _mat The material assigned to this ellipsoid - public: void SetMat(const Material &_mat); - - /// \brief Get the mass matrix for this ellipsoid. This function - /// is only meaningful if the ellipsoid's radii and material - /// have been set. - /// \return The computed mass matrix if parameters are valid - /// (radius > 0), (length > 0), and (density > 0). Otherwise - /// std::nullopt is returned. - public: std::optional< MassMatrix3 > MassMatrix() const; - - /// \brief Check if this ellipsoid is equal to the provided ellipsoid. - /// Radius, length, and material properties will be checked. - public: bool operator==(const Ellipsoid &_ellipsoid) const; - - /// \brief Get the volume of the ellipsoid in m^3. - /// \return Volume of the ellipsoid in m^3. - public: Precision Volume() const; - - /// \brief Compute the ellipsoid's density given a mass value. The - /// ellipsoid is assumed to be solid with uniform density. This - /// function requires the ellipsoid's radius and length to be set to - /// values greater than zero. The Material of the ellipsoid is ignored. - /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be - /// greater than zero. - /// \return Density of the ellipsoid in kg/m^3. A NaN is returned - /// if radius, length or _mass is <= 0. - public: Precision DensityFromMass(const Precision _mass) const; - - /// \brief Set the density of this ellipsoid based on a mass value. - /// Density is computed using - /// Precision DensityFromMass(const Precision _mass) const. The - /// ellipsoid is assumed to be solid with uniform density. This - /// function requires the ellipsoid's radius and length to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the ellipsoid, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// ellipsoid's radius, length, or the _mass value are <= 0. - /// \sa Precision DensityFromMass(const Precision _mass) const - public: bool SetDensityFromMass(const Precision _mass); - - /// \brief Radius of the ellipsoid. - private: Vector3 radii = Vector3::Zero; - - /// \brief the ellipsoid's material. - private: Material material; - }; - - /// \typedef Ellipsoid Ellipsoidi - /// \brief Ellipsoid with integer precision. - typedef Ellipsoid Ellipsoidi; - - /// \typedef Ellipsoid Ellipsoidd - /// \brief Ellipsoid with double precision. - typedef Ellipsoid Ellipsoidd; - - /// \typedef Ellipsoid Ellipsoidf - /// \brief Ellipsoid with float precision. - typedef Ellipsoid Ellipsoidf; - } - } -} -#include "ignition/math/detail/Ellipsoid.hh" - -#endif +#include +#include diff --git a/include/ignition/math/Export.hh b/include/ignition/math/Export.hh new file mode 100644 index 000000000..04084226f --- /dev/null +++ b/include/ignition/math/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/math/Filter.hh b/include/ignition/math/Filter.hh index f88d3c8d9..636aa4caf 100644 --- a/include/ignition/math/Filter.hh +++ b/include/ignition/math/Filter.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Open Source Robotics Foundation + * 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. @@ -13,240 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_FILTER_HH_ -#define IGNITION_MATH_FILTER_HH_ + */ -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Filter Filter.hh ignition/math/Filter.hh - /// \brief Filter base class - template - class Filter - { - /// \brief Destructor. - public: virtual ~Filter() {} - - /// \brief Set the output of the filter. - /// \param[in] _val New value. - public: virtual void Set(const T &_val) - { - y0 = _val; - } - - /// \brief Set the cutoff frequency and sample rate. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: virtual void Fc(double _fc, double _fs) = 0; - - /// \brief Get the output of the filter. - /// \return Filter's output. - public: virtual const T &Value() const - { - return y0; - } - - /// \brief Output. - protected: T y0{}; - }; - - /// \class OnePole Filter.hh ignition/math/Filter.hh - /// \brief A one-pole DSP filter. - /// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/ - template - class OnePole : public Filter - { - /// \brief Constructor. - public: OnePole() = default; - - /// \brief Constructor. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: OnePole(double _fc, double _fs) - { - this->Fc(_fc, _fs); - } - - // Documentation Inherited. - public: virtual void Fc(double _fc, double _fs) override - { - b1 = exp(-2.0 * IGN_PI * _fc / _fs); - a0 = 1.0 - b1; - } - - /// \brief Update the filter's output. - /// \param[in] _x Input value. - /// \return The filter's current output. - public: const T& Process(const T &_x) - { - this->y0 = a0 * _x + b1 * this->y0; - return this->y0; - } - - /// \brief Input gain control. - protected: double a0 = 0; - - /// \brief Gain of the feedback. - protected: double b1 = 0; - }; - - /// \class OnePoleQuaternion Filter.hh ignition/math/Filter.hh - /// \brief One-pole quaternion filter. - class OnePoleQuaternion : public OnePole - { - /// \brief Constructor. - public: OnePoleQuaternion() - { - this->Set(math::Quaterniond(1, 0, 0, 0)); - } - - /// \brief Constructor. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: OnePoleQuaternion(double _fc, double _fs) - : OnePole(_fc, _fs) - { - this->Set(math::Quaterniond(1, 0, 0, 0)); - } - - /// \brief Update the filter's output. - /// \param[in] _x Input value. - /// \return The filter's current output. - public: const math::Quaterniond& Process( - const math::Quaterniond &_x) - { - y0 = math::Quaterniond::Slerp(a0, y0, _x); - return y0; - } - }; - - /// \class OnePoleVector3 Filter.hh ignition/math/Filter.hh - /// \brief One-pole vector3 filter. - class OnePoleVector3 : public OnePole - { - /// \brief Constructor. - public: OnePoleVector3() - { - this->Set(math::Vector3d(0, 0, 0)); - } - - /// \brief Constructor. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: OnePoleVector3(double _fc, double _fs) - : OnePole(_fc, _fs) - { - this->Set(math::Vector3d(0, 0, 0)); - } - }; - - /// \class BiQuad Filter.hh ignition/math/Filter.hh - /// \brief Bi-quad filter base class. - /// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/ - template - class BiQuad : public Filter - { - /// \brief Constructor. - public: BiQuad() = default; - - /// \brief Constructor. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: BiQuad(double _fc, double _fs) - { - this->Fc(_fc, _fs); - } - - // Documentation Inherited. - public: void Fc(double _fc, double _fs) override - { - this->Fc(_fc, _fs, 0.5); - } - - /// \brief Set the cutoff frequency, sample rate and Q coefficient. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - /// \param[in] _q Q coefficient. - public: void Fc(double _fc, double _fs, double _q) - { - double k = tan(IGN_PI * _fc / _fs); - double kQuadDenom = k * k + k / _q + 1.0; - this->a0 = k * k/ kQuadDenom; - this->a1 = 2 * this->a0; - this->a2 = this->a0; - this->b0 = 1.0; - this->b1 = 2 * (k * k - 1.0) / kQuadDenom; - this->b2 = (k * k - k / _q + 1.0) / kQuadDenom; - } - - /// \brief Set the current filter's output. - /// \param[in] _val New filter's output. - public: virtual void Set(const T &_val) override - { - this->y0 = this->y1 = this->y2 = this->x1 = this->x2 = _val; - } - - /// \brief Update the filter's output. - /// \param[in] _x Input value. - /// \return The filter's current output. - public: virtual const T& Process(const T &_x) - { - this->y0 = this->a0 * _x + - this->a1 * this->x1 + - this->a2 * this->x2 - - this->b1 * this->y1 - - this->b2 * this->y2; - - this->x2 = this->x1; - this->x1 = _x; - this->y2 = this->y1; - this->y1 = this->y0; - return this->y0; - } - - /// \brief Input gain control coefficients. - protected: double a0 = 0, - a1 = 0, - a2 = 0, - b0 = 0, - b1 = 0, - b2 = 0; - - /// \brief Gain of the feedback coefficients. - protected: T x1{}, x2{}, y1{}, y2{}; - }; - - /// \class BiQuadVector3 Filter.hh ignition/math/Filter.hh - /// \brief BiQuad vector3 filter - class BiQuadVector3 : public BiQuad - { - /// \brief Constructor. - public: BiQuadVector3() - { - this->Set(math::Vector3d(0, 0, 0)); - } - - /// \brief Constructor. - /// \param[in] _fc Cutoff frequency. - /// \param[in] _fs Sample rate. - public: BiQuadVector3(double _fc, double _fs) - : BiQuad(_fc, _fs) - { - this->Set(math::Vector3d(0, 0, 0)); - } - }; - } - } -} - -#endif diff --git a/include/ignition/math/Frustum.hh b/include/ignition/math/Frustum.hh index d297490cd..3b089d8ee 100644 --- a/include/ignition/math/Frustum.hh +++ b/include/ignition/math/Frustum.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Open Source Robotics Foundation + * 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. @@ -13,175 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_FRUSTUM_HH_ -#define IGNITION_MATH_FRUSTUM_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declaration of private data - class FrustumPrivate; - - /// \brief Mathematical representation of a frustum and related functions. - /// This is also known as a view frustum. - class IGNITION_MATH_VISIBLE Frustum - { - /// \brief Planes that define the boundaries of the frustum. - public: enum FrustumPlane - { - /// \brief Near plane - FRUSTUM_PLANE_NEAR = 0, - - /// \brief Far plane - FRUSTUM_PLANE_FAR = 1, - - /// \brief Left plane - FRUSTUM_PLANE_LEFT = 2, - - /// \brief Right plane - FRUSTUM_PLANE_RIGHT = 3, - - /// \brief Top plane - FRUSTUM_PLANE_TOP = 4, - - /// \brief Bottom plane - FRUSTUM_PLANE_BOTTOM = 5 - }; - - /// \brief Default constructor. With the following default values: - /// - /// * near: 0.0 - /// * far: 1.0 - /// * fov: 0.78539 radians (45 degrees) - /// * aspect ratio: 1.0 - /// * pose: Pose3d::Zero - public: Frustum(); - - /// \brief Constructor - /// \param[in] _near Near plane distance. This is the distance from - /// the frustum's vertex to the closest plane - /// \param[in] _far Far plane distance. This is the distance from the - /// frustum's vertex to the farthest plane. - /// \param[in] _fov Field of view. The field of view is the - /// angle between the frustum's vertex and the edges of the near or far - /// plane. This value represents the horizontal angle. - /// \param[in] _aspectRatio The aspect ratio, which is the width divided - /// by height of the near or far planes. - /// \param[in] _pose Pose of the frustum, which is the vertex (top of - /// the pyramid). - public: Frustum(const double _near, - const double _far, - const math::Angle &_fov, - const double _aspectRatio, - const math::Pose3d &_pose = math::Pose3d::Zero); - - /// \brief Copy Constructor - /// \param[in] _p Frustum to copy. - public: Frustum(const Frustum &_p); - - /// \brief Destructor - public: virtual ~Frustum(); - - /// \brief Get the near distance. This is the distance from the - /// frustum's vertex to the closest plane. - /// \return Near distance. - /// \sa SetNear - public: double Near() const; - - /// \brief Set the near distance. This is the distance from the - /// frustum's vertex to the closest plane. - /// \param[in] _near Near distance. - /// \sa Near - public: void SetNear(const double _near); - - /// \brief Get the far distance. This is the distance from the - /// frustum's vertex to the farthest plane. - /// \return Far distance. - /// \sa SetFar - public: double Far() const; - - /// \brief Set the far distance. This is the distance from the - /// frustum's vertex to the farthest plane. - /// \param[in] _far Far distance. - /// \sa Far - public: void SetFar(const double _far); - - /// \brief Get the horizontal field of view. The field of view is the - /// angle between the frustum's vertex and the edges of the near or far - /// plane. This value represents the horizontal angle. - /// \return The field of view. - /// \sa SetFOV - public: math::Angle FOV() const; - - /// \brief Set the horizontal field of view. The field of view is the - /// angle between the frustum's vertex and the edges of the near or far - /// plane. This value represents the horizontal angle. - /// \param[in] _fov The field of view. - /// \sa FOV - public: void SetFOV(const math::Angle &_fov); - - /// \brief Get the aspect ratio, which is the width divided by height - /// of the near or far planes. - /// \return The frustum's aspect ratio. - /// \sa SetAspectRatio - public: double AspectRatio() const; - - /// \brief Set the aspect ratio, which is the width divided by height - /// of the near or far planes. - /// \param[in] _aspectRatio The frustum's aspect ratio. - /// \sa AspectRatio - public: void SetAspectRatio(const double _aspectRatio); - - /// \brief Get a plane of the frustum. - /// \param[in] _plane The plane to return. - /// \return Plane of the frustum. - public: Planed Plane(const FrustumPlane _plane) const; - - /// \brief Check if a box lies inside the pyramid frustum. - /// \param[in] _b Box to check. - /// \return True if the box is inside the pyramid frustum. - public: bool Contains(const AxisAlignedBox &_b) const; - - /// \brief Check if a point lies inside the pyramid frustum. - /// \param[in] _p Point to check. - /// \return True if the point is inside the pyramid frustum. - public: bool Contains(const Vector3d &_p) const; - - /// \brief Get the pose of the frustum - /// \return Pose of the frustum - /// \sa SetPose - public: Pose3d Pose() const; - - /// \brief Set the pose of the frustum - /// \param[in] _pose Pose of the frustum, top vertex. - /// \sa Pose - public: void SetPose(const Pose3d &_pose); - - /// \brief Assignment operator. Set this frustum to the parameter. - /// \param[in] _f Frustum to copy - /// \return The new frustum. - public: Frustum &operator=(const Frustum &_f); - - /// \brief Compute the planes of the frustum. This is called whenever - /// a property of the frustum is changed. - private: void ComputePlanes(); - - /// \internal - /// \brief Private data pointer - private: FrustumPrivate *dataPtr; - }; - } - } -} -#endif diff --git a/include/ignition/math/GaussMarkovProcess.hh b/include/ignition/math/GaussMarkovProcess.hh index 02ebb1a4f..7c9802c45 100644 --- a/include/ignition/math/GaussMarkovProcess.hh +++ b/include/ignition/math/GaussMarkovProcess.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. @@ -13,141 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_GAUSSMARKOVPROCESS_HH_ -#define IGNITION_MATH_GAUSSMARKOVPROCESS_HH_ + */ -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Use a steady clock - using clock = std::chrono::steady_clock; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declarations. - class GaussMarkovProcessPrivate; - - /** \class GaussMarkovProcess GaussMarkovProcess.hh\ - * ignition/math/GaussMarkovProcess.hh - **/ - /// \brief Implementation of a stationary gauss-markov process, also - /// known as a Ornstein Ulenbeck process. - /// - /// See the Update(const clock::duration &) for details on the forumla - /// used to update the process. - /// - /// ## Example usage - /// - /// \snippet examples/gauss_markov_process_example.cc complete - class IGNITION_MATH_VISIBLE GaussMarkovProcess - { - // Default constructor. This sets all the parameters to zero. - public: GaussMarkovProcess(); - - /// \brief Create a process with the provided process parameters. - /// This will also call Set(), and in turn Reset(). - /// \param[in] _start The start value of the process. - /// \param[in] _theta The theta (\f$\theta\f$) parameter. A value of - /// zero will be used if this parameter is negative. - /// \param[in] _mu The mu (\f$\mu\f$) parameter. - /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. A value of - /// zero will be used if this parameter is negative. - /// \sa Update(const clock::duration &) - public: GaussMarkovProcess(double _start, double _theta, double _mu, - double _sigma); - - /// \brief Destructor. - public: ~GaussMarkovProcess(); - - /// \brief Set the process parameters. This will also call Reset(). - /// \param[in] _start The start value of the process. - /// \param[in] _theta The theta (\f$\theta\f$) parameter. - /// \param[in] _mu The mu (\f$\mu\f$) parameter. - /// \param[in] _sigma The sigma (\f$\sigma\f$) parameter. - /// \sa Update(const clock::duration &) - public: void Set(double _start, double _theta, double _mu, double _sigma); - - /// \brief Get the start value. - /// \return The start value. - /// \sa Set(double, double, double, double) - public: double Start() const; - - /// \brief Get the current process value. - /// \return The value of the process. - public: double Value() const; - - /// \brief Get the theta (\f$\theta\f$) value. - /// \return The theta value. - /// \sa Set(double, double, double, double) - public: double Theta() const; - - /// \brief Get the mu (\f$\mu\f$) value. - /// \return The mu value. - /// \sa Set(double, double, double, double) - public: double Mu() const; - - /// \brief Get the sigma (\f$\sigma\f$) value. - /// \return The sigma value. - /// \sa Set(double, double, double, double) - public: double Sigma() const; - - /// \brief Reset the process. This will set the current process value - /// to the start value. - public: void Reset(); - - /// \brief Update the process and get the new value. - /// - /// The following equation is computed: - /// - /// \f$x_{t+1} += \theta * (\mu - x_t) * dt + \sigma * dW_t\f$ - /// - /// where - /// - /// * \f$\theta, \mu, \sigma\f$ are parameters specified by the - /// user. In order, the parameters are theta, mu, and sigma. Theta - /// and sigma must be greater than or equal to zero. You can think - /// of mu as representing the mean or equilibrium value, sigma as the - /// degree of volatility, and theta as the rate by which changes - /// dissipate and revert towards the mean. - /// * \f$dt\f$ is the time step in seconds. - /// * \f$dW_t\f$ is a random number drawm from a normal distribution - /// with mean of zero and variance of 1. - /// * \f$x_t\f$ is the current value of the Gauss-Markov process - /// * \f$x_{t+1}\f$ is the new value of the Gauss-Markvov process - /// - /// See also: https://en.wikipedia.org/wiki/Ornstein%E2%80%93Uhlenbeck_process - /// - /// This implementation include a drift parameter, mu. In financial - /// mathematics, this is known as a Vasicek model. - /// - /// \param[in] _dt Length of the timestep after which a new sample - /// should be taken. - /// \return The new value of this process. - public: double Update(const clock::duration &_dt); - - public: double Update(double _dt); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif diff --git a/include/ignition/math/Helpers.hh b/include/ignition/math/Helpers.hh index 5d69b209d..e140adc89 100644 --- a/include/ignition/math/Helpers.hh +++ b/include/ignition/math/Helpers.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,1162 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_FUNCTIONS_HH_ -#define IGNITION_MATH_FUNCTIONS_HH_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + */ +#include #include -#include "ignition/math/Export.hh" - -/// \brief The default tolerance value used by MassMatrix3::IsValid(), -/// MassMatrix3::IsPositive(), and MassMatrix3::ValidMoments() -template -constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); - -/// \brief Double maximum value. This value will be similar to 1.79769e+308 -/// \deprecated Use static const value instead. -#define IGN_DBL_MAX ignition::math::DPRCT_MAX_D - -/// \brief Double min value. This value will be similar to 2.22507e-308 -/// \deprecated Use static const value instead. -#define IGN_DBL_MIN ignition::math::DPRCT_MIN_D - -/// \brief Double low value, equivalent to -IGN_DBL_MAX -/// \deprecated Use static const value instead. -#define IGN_DBL_LOW ignition::math::DPRCT_LOW_D - -/// \brief Double positive infinite value -/// \deprecated Use static const value instead. -#define IGN_DBL_INF ignition::math::DPRCT_INF_D - -/// \brief Float maximum value. This value will be similar to 3.40282e+38 -/// \deprecated Use static const value instead. -#define IGN_FLT_MAX ignition::math::DPRCT_MAX_F - -/// \brief Float minimum value. This value will be similar to 1.17549e-38 -/// \deprecated Use static const value instead. -#define IGN_FLT_MIN ignition::math::DPRCT_MIN_F - -/// \brief Float lowest value, equivalent to -IGN_FLT_MAX -/// \deprecated Use static const value instead. -#define IGN_FLT_LOW ignition::math::DPRCT_LOW_F - -/// \brief Float positive infinite value -/// \deprecated Use static const value instead. -#define IGN_FLT_INF ignition::math::DPRCT_INF_F - -/// \brief 16bit unsigned integer maximum value -/// \deprecated Use static const value instead. -#define IGN_UINT16_MAX ignition::math::DPRCT_MAX_UI16 - -/// \brief 16bit unsigned integer minimum value -/// \deprecated Use static const value instead. -#define IGN_UINT16_MIN ignition::math::DPRCT_MIN_UI16 - -/// \brief 16bit unsigned integer lowest value. This is equivalent to -/// IGN_UINT16_MIN, and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_UINT16_LOW ignition::math::DPRCT_LOW_UI16 - -/// \brief 16-bit unsigned integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_UINT16_INF ignition::math::DPRCT_INF_UI16 - -/// \brief 16bit integer maximum value -/// \deprecated Use static const value instead. -#define IGN_INT16_MAX ignition::math::DPRCT_MAX_I16 - -/// \brief 16bit integer minimum value -/// \deprecated Use static const value instead. -#define IGN_INT16_MIN ignition::math::DPRCT_MIN_I16 - -/// \brief 16bit integer lowest value. This is equivalent to IGN_INT16_MIN, -/// and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_INT16_LOW ignition::math::DPRCT_LOW_I16 - -/// \brief 16-bit integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_INT16_INF ignition::math::DPRCT_INF_I16 - -/// \brief 32bit unsigned integer maximum value -/// \deprecated Use static const value instead. -#define IGN_UINT32_MAX ignition::math::DPRCT_MAX_UI32 - -/// \brief 32bit unsigned integer minimum value -/// \deprecated Use static const value instead. -#define IGN_UINT32_MIN ignition::math::DPRCT_MIN_UI32 - -/// \brief 32bit unsigned integer lowest value. This is equivalent to -/// IGN_UINT32_MIN, and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_UINT32_LOW ignition::math::DPRCT_LOW_UI32 - -/// \brief 32-bit unsigned integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_UINT32_INF ignition::math::DPRCT_INF_UI32 - -/// \brief 32bit integer maximum value -/// \deprecated Use static const value instead. -#define IGN_INT32_MAX ignition::math::DPRCT_MAX_I32 - -/// \brief 32bit integer minimum value -/// \deprecated Use static const value instead. -#define IGN_INT32_MIN ignition::math::DPRCT_MIN_I32 - -/// \brief 32bit integer minimum value. This is equivalent to IGN_INT32_MIN, -/// and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_INT32_LOW ignition::math::DPRCT_LOW_I32 - -/// \brief 32-bit integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_INT32_INF ignition::math::DPRCT_INF_I32 - -/// \brief 64bit unsigned integer maximum value -/// \deprecated Use static const value instead. -#define IGN_UINT64_MAX ignition::math::DPRCT_MAX_UI64 - -/// \brief 64bit unsigned integer minimum value -/// \deprecated Use static const value instead. -#define IGN_UINT64_MIN ignition::math::DPRCT_MIN_UI64 - -/// \brief 64bit unsigned integer lowest value. This is equivalent to -/// IGN_UINT64_MIN, and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_UINT64_LOW ignition::math::DPRCT_LOW_UI64 - -/// \brief 64-bit unsigned integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_UINT64_INF ignition::math::DPRCT_INF_UI64 - -/// \brief 64bit integer maximum value -/// \deprecated Use static const value instead. -#define IGN_INT64_MAX ignition::math::DPRCT_MAX_I64 - -/// \brief 64bit integer minimum value -/// \deprecated Use static const value instead. -#define IGN_INT64_MIN ignition::math::DPRCT_MIN_I64 - -/// \brief 64bit integer lowest value. This is equivalent to IGN_INT64_MIN, -/// and is defined here for completeness. -/// \deprecated Use static const value instead. -#define IGN_INT64_LOW ignition::math::DPRCT_LOW_I64 - -/// \brief 64-bit integer positive infinite value -/// \deprecated Use static const value instead. -#define IGN_INT64_INF ignition::math::DPRCT_INF_I64 - -/// \brief Define IGN_PI, IGN_PI_2, and IGN_PI_4. -/// This was put here for Windows support. -#ifdef M_PI -#define IGN_PI M_PI -#define IGN_PI_2 M_PI_2 -#define IGN_PI_4 M_PI_4 -#define IGN_SQRT2 M_SQRT2 -#else -#define IGN_PI 3.14159265358979323846 -#define IGN_PI_2 1.57079632679489661923 -#define IGN_PI_4 0.78539816339744830962 -#define IGN_SQRT2 1.41421356237309504880 -#endif - -/// \brief Define IGN_FP_VOLATILE for FP equality comparisons -/// Use volatile parameters when checking floating point equality on -/// the 387 math coprocessor to work around bugs from the 387 extra precision -#if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2 -#define IGN_FP_VOLATILE volatile -#else -#define IGN_FP_VOLATILE -#endif - -/// \brief Compute sphere volume -/// \param[in] _radius Sphere radius -#define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0) - -/// \brief Compute cylinder volume -/// \param[in] _r Cylinder base radius -/// \param[in] _l Cylinder length -#define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2)) - -/// \brief Compute box volume -/// \param[in] _x X length -/// \param[in] _y Y length -/// \param[in] _z Z length -#define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z) - -/// \brief Compute box volume from a vector -/// \param[in] _v Vector3d that contains the box's dimensions. -#define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z()) - -namespace ignition -{ - /// \brief Math classes and function useful in robot applications. - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \brief size_t type with a value of 0 - static const size_t IGN_ZERO_SIZE_T = 0u; - - /// \brief size_t type with a value of 1 - static const size_t IGN_ONE_SIZE_T = 1u; - - /// \brief size_t type with a value of 2 - static const size_t IGN_TWO_SIZE_T = 2u; - - /// \brief size_t type with a value of 3 - static const size_t IGN_THREE_SIZE_T = 3u; - - /// \brief size_t type with a value of 4 - static const size_t IGN_FOUR_SIZE_T = 4u; - - /// \brief size_t type with a value of 5 - static const size_t IGN_FIVE_SIZE_T = 5u; - - /// \brief size_t type with a value of 6 - static const size_t IGN_SIX_SIZE_T = 6u; - - /// \brief size_t type with a value of 7 - static const size_t IGN_SEVEN_SIZE_T = 7u; - - /// \brief size_t type with a value of 8 - static const size_t IGN_EIGHT_SIZE_T = 8u; - - /// \brief size_t type with a value of 9 - static const size_t IGN_NINE_SIZE_T = 9u; - - /// \brief Double maximum value. This value will be similar to 1.79769e+308 - static const double MAX_D = std::numeric_limits::max(); - - /// \brief Double min value. This value will be similar to 2.22507e-308 - static const double MIN_D = std::numeric_limits::min(); - - /// \brief Double low value, equivalent to -MAX_D - static const double LOW_D = std::numeric_limits::lowest(); - - /// \brief Double positive infinite value - static const double INF_D = std::numeric_limits::infinity(); - - /// \brief Returns the representation of a quiet not a number (NAN) - static const double NAN_D = std::numeric_limits::quiet_NaN(); - - /// \brief Float maximum value. This value will be similar to 3.40282e+38 - static const float MAX_F = std::numeric_limits::max(); - - /// \brief Float minimum value. This value will be similar to 1.17549e-38 - static const float MIN_F = std::numeric_limits::min(); - - /// \brief Float low value, equivalent to -MAX_F - static const float LOW_F = std::numeric_limits::lowest(); - - /// \brief float positive infinite value - static const float INF_F = std::numeric_limits::infinity(); - - /// \brief Returns the representation of a quiet not a number (NAN) - static const float NAN_F = std::numeric_limits::quiet_NaN(); - - /// \brief 16bit unsigned integer maximum value - static const uint16_t MAX_UI16 = std::numeric_limits::max(); - - /// \brief 16bit unsigned integer minimum value - static const uint16_t MIN_UI16 = std::numeric_limits::min(); - - /// \brief 16bit unsigned integer lowest value. This is equivalent to - /// IGN_UINT16_MIN, and is defined here for completeness. - static const uint16_t LOW_UI16 = std::numeric_limits::lowest(); - - /// \brief 16-bit unsigned integer positive infinite value - static const uint16_t INF_UI16 = std::numeric_limits::infinity(); - - /// \brief 16bit unsigned integer maximum value - static const int16_t MAX_I16 = std::numeric_limits::max(); - - /// \brief 16bit unsigned integer minimum value - static const int16_t MIN_I16 = std::numeric_limits::min(); - - /// \brief 16bit unsigned integer lowest value. This is equivalent to - /// IGN_INT16_MIN, and is defined here for completeness. - static const int16_t LOW_I16 = std::numeric_limits::lowest(); - - /// \brief 16-bit unsigned integer positive infinite value - static const int16_t INF_I16 = std::numeric_limits::infinity(); - - /// \brief 32bit unsigned integer maximum value - static const uint32_t MAX_UI32 = std::numeric_limits::max(); - - /// \brief 32bit unsigned integer minimum value - static const uint32_t MIN_UI32 = std::numeric_limits::min(); - - /// \brief 32bit unsigned integer lowest value. This is equivalent to - /// IGN_UINT32_MIN, and is defined here for completeness. - static const uint32_t LOW_UI32 = std::numeric_limits::lowest(); - - /// \brief 32-bit unsigned integer positive infinite value - static const uint32_t INF_UI32 = std::numeric_limits::infinity(); - - /// \brief 32bit unsigned integer maximum value - static const int32_t MAX_I32 = std::numeric_limits::max(); - - /// \brief 32bit unsigned integer minimum value - static const int32_t MIN_I32 = std::numeric_limits::min(); - - /// \brief 32bit unsigned integer lowest value. This is equivalent to - /// IGN_INT32_MIN, and is defined here for completeness. - static const int32_t LOW_I32 = std::numeric_limits::lowest(); - - /// \brief 32-bit unsigned integer positive infinite value - static const int32_t INF_I32 = std::numeric_limits::infinity(); - - /// \brief 64bit unsigned integer maximum value - static const uint64_t MAX_UI64 = std::numeric_limits::max(); - - /// \brief 64bit unsigned integer minimum value - static const uint64_t MIN_UI64 = std::numeric_limits::min(); - - /// \brief 64bit unsigned integer lowest value. This is equivalent to - /// IGN_UINT64_MIN, and is defined here for completeness. - static const uint64_t LOW_UI64 = std::numeric_limits::lowest(); - - /// \brief 64-bit unsigned integer positive infinite value - static const uint64_t INF_UI64 = std::numeric_limits::infinity(); - - /// \brief 64bit unsigned integer maximum value - static const int64_t MAX_I64 = std::numeric_limits::max(); - - /// \brief 64bit unsigned integer minimum value - static const int64_t MIN_I64 = std::numeric_limits::min(); - - /// \brief 64bit unsigned integer lowest value. This is equivalent to - /// IGN_INT64_MIN, and is defined here for completeness. - static const int64_t LOW_I64 = std::numeric_limits::lowest(); - - /// \brief 64-bit unsigned integer positive infinite value - static const int64_t INF_I64 = std::numeric_limits::infinity(); - - /// \brief Returns the representation of a quiet not a number (NAN) - static const int NAN_I = std::numeric_limits::quiet_NaN(); - - // variables created to deprecate macros in this file - static const double IGN_DEPRECATED(3) DPRCT_MAX_D = MAX_D; - static const double IGN_DEPRECATED(3) DPRCT_MIN_D = MIN_D; - static const double IGN_DEPRECATED(3) DPRCT_LOW_D = LOW_D; - static const double IGN_DEPRECATED(3) DPRCT_INF_D = INF_D; - static const float IGN_DEPRECATED(3) DPRCT_MAX_F = MAX_F; - static const float IGN_DEPRECATED(3) DPRCT_MIN_F = MIN_F; - static const float IGN_DEPRECATED(3) DPRCT_LOW_F = LOW_F; - static const float IGN_DEPRECATED(3) DPRCT_INF_F = INF_F; - static const uint16_t IGN_DEPRECATED(3) DPRCT_MAX_UI16 = MAX_UI16; - static const uint16_t IGN_DEPRECATED(3) DPRCT_MIN_UI16 = MIN_UI16; - static const uint16_t IGN_DEPRECATED(3) DPRCT_LOW_UI16 = LOW_UI16; - static const uint16_t IGN_DEPRECATED(3) DPRCT_INF_UI16 = INF_UI16; - static const int16_t IGN_DEPRECATED(3) DPRCT_MAX_I16 = MAX_I16; - static const int16_t IGN_DEPRECATED(3) DPRCT_MIN_I16 = MIN_I16; - static const int16_t IGN_DEPRECATED(3) DPRCT_LOW_I16 = LOW_I16; - static const int16_t IGN_DEPRECATED(3) DPRCT_INF_I16 = INF_I16; - static const uint32_t IGN_DEPRECATED(3) DPRCT_MAX_UI32 = MAX_UI32; - static const uint32_t IGN_DEPRECATED(3) DPRCT_MIN_UI32 = MIN_UI32; - static const uint32_t IGN_DEPRECATED(3) DPRCT_LOW_UI32 = LOW_UI32; - static const uint32_t IGN_DEPRECATED(3) DPRCT_INF_UI32 = INF_UI32; - static const int32_t IGN_DEPRECATED(3) DPRCT_MAX_I32 = MAX_I32; - static const int32_t IGN_DEPRECATED(3) DPRCT_MIN_I32 = MIN_I32; - static const int32_t IGN_DEPRECATED(3) DPRCT_LOW_I32 = LOW_I32; - static const int32_t IGN_DEPRECATED(3) DPRCT_INF_I32 = INF_I32; - static const uint64_t IGN_DEPRECATED(3) DPRCT_MAX_UI64 = MAX_UI64; - static const uint64_t IGN_DEPRECATED(3) DPRCT_MIN_UI64 = MIN_UI64; - static const uint64_t IGN_DEPRECATED(3) DPRCT_LOW_UI64 = LOW_UI64; - static const uint64_t IGN_DEPRECATED(3) DPRCT_INF_UI64 = INF_UI64; - static const int64_t IGN_DEPRECATED(3) DPRCT_MAX_I64 = MAX_I64; - static const int64_t IGN_DEPRECATED(3) DPRCT_MIN_I64 = MIN_I64; - static const int64_t IGN_DEPRECATED(3) DPRCT_LOW_I64 = LOW_I64; - static const int64_t IGN_DEPRECATED(3) DPRCT_INF_I64 = INF_I64; - - /// \brief Simple clamping function - /// \param[in] _v value - /// \param[in] _min minimum - /// \param[in] _max maximum - template - inline T clamp(T _v, T _min, T _max) - { - return std::max(std::min(_v, _max), _min); - } - - /// \brief check if a float is NaN - /// \param[in] _v the value - /// \return true if _v is not a number, false otherwise - inline bool isnan(float _v) - { - return (std::isnan)(_v); - } - - /// \brief check if a double is NaN - /// \param[in] _v the value - /// \return true if _v is not a number, false otherwise - inline bool isnan(double _v) - { - return (std::isnan)(_v); - } - - /// \brief Fix a nan value. - /// \param[in] _v Value to correct. - /// \return 0 if _v is NaN, _v otherwise. - inline float fixnan(float _v) - { - return isnan(_v) || std::isinf(_v) ? 0.0f : _v; - } - - /// \brief Fix a nan value. - /// \param[in] _v Value to correct. - /// \return 0 if _v is NaN, _v otherwise. - inline double fixnan(double _v) - { - return isnan(_v) || std::isinf(_v) ? 0.0 : _v; - } - - /// \brief Check if parameter is even. - /// \param[in] _v Value to check. - /// \return True if _v is even. - inline bool isEven(const int _v) - { - return !(_v % 2); - } - - /// \brief Check if parameter is even. - /// \param[in] _v Value to check. - /// \return True if _v is even. - inline bool isEven(const unsigned int _v) - { - return !(_v % 2); - } - - /// \brief Check if parameter is odd. - /// \param[in] _v Value to check. - /// \return True if _v is odd. - inline bool isOdd(const int _v) - { - return (_v % 2) != 0; - } - - /// \brief Check if parameter is odd. - /// \param[in] _v Value to check. - /// \return True if _v is odd. - inline bool isOdd(const unsigned int _v) - { - return (_v % 2) != 0; - } - - /// \brief The signum function. - /// - /// Returns 0 for zero values, -1 for negative values, - /// +1 for positive values. - /// \param[in] _value The value. - /// \return The signum of the value. - template - inline int sgn(T _value) - { - return (T(0) < _value) - (_value < T(0)); - } - - /// \brief The signum function. - /// - /// Returns 0 for zero values, -1 for negative values, - /// +1 for positive values. - /// \param[in] _value The value. - /// \return The signum of the value. - template - inline int signum(T _value) - { - return sgn(_value); - } - - /// \brief get mean of vector of values - /// \param[in] _values the vector of values - /// \return the mean - template - inline T mean(const std::vector &_values) - { - T sum = 0; - for (unsigned int i = 0; i < _values.size(); ++i) - sum += _values[i]; - return sum / _values.size(); - } - - /// \brief get variance of vector of values - /// \param[in] _values the vector of values - /// \return the squared deviation - template - inline T variance(const std::vector &_values) - { - T avg = mean(_values); - - T sum = 0; - for (unsigned int i = 0; i < _values.size(); ++i) - sum += (_values[i] - avg) * (_values[i] - avg); - return sum / _values.size(); - } - - /// \brief get the maximum value of vector of values - /// \param[in] _values the vector of values - /// \return maximum - template - inline T max(const std::vector &_values) - { - T max = std::numeric_limits::min(); - for (unsigned int i = 0; i < _values.size(); ++i) - if (_values[i] > max) - max = _values[i]; - return max; - } - - /// \brief get the minimum value of vector of values - /// \param[in] _values the vector of values - /// \return minimum - template - inline T min(const std::vector &_values) - { - T min = std::numeric_limits::max(); - for (unsigned int i = 0; i < _values.size(); ++i) - if (_values[i] < min) - min = _values[i]; - return min; - } - - /// \brief check if two values are equal, within a tolerance - /// \param[in] _a the first value - /// \param[in] _b the second value - /// \param[in] _epsilon the tolerance - template - inline bool equal(const T &_a, const T &_b, - const T &_epsilon = T(1e-6)) - { - IGN_FP_VOLATILE T diff = std::abs(_a - _b); - return diff <= _epsilon; - } - - /// \brief inequality test, within a tolerance - /// \param[in] _a the first value - /// \param[in] _b the second value - /// \param[in] _epsilon the tolerance - template - inline bool lessOrNearEqual(const T &_a, const T &_b, - const T &_epsilon = 1e-6) - { - return _a < _b + _epsilon; - } - - /// \brief inequality test, within a tolerance - /// \param[in] _a the first value - /// \param[in] _b the second value - /// \param[in] _epsilon the tolerance - template - inline bool greaterOrNearEqual(const T &_a, const T &_b, - const T &_epsilon = 1e-6) - { - return _a > _b - _epsilon; - } - - /// \brief get value at a specified precision - /// \param[in] _a the number - /// \param[in] _precision the precision - /// \return the value for the specified precision - template - inline T precision(const T &_a, const unsigned int &_precision) - { - auto p = std::pow(10, _precision); - return static_cast(std::round(_a * p) / p); - } - - /// \brief Sort two numbers, such that _a <= _b - /// \param[out] _a the first number - /// \param[out] _b the second number - template - inline void sort2(T &_a, T &_b) - { - using std::swap; - if (_b < _a) - swap(_a, _b); - } - - /// \brief Sort three numbers, such that _a <= _b <= _c - /// \param[out] _a the first number - /// \param[out] _b the second number - /// \param[out] _c the third number - template - inline void sort3(T &_a, T &_b, T &_c) - { - // _a <= _b - sort2(_a, _b); - // _a <= _c, _b <= _c - sort2(_b, _c); - // _a <= _b <= _c - 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 - inline bool isPowerOfTwo(unsigned int _x) - { - return ((_x != 0) && ((_x & (~_x + 1)) == _x)); - } - - /// \brief Get the smallest power of two that is greater or equal to - /// a given value - /// \param[in] _x the number - /// \return the same value if _x is already a power of two. Otherwise, - /// it returns the smallest power of two that is greater than _x - inline unsigned int roundUpPowerOfTwo(unsigned int _x) - { - if (_x == 0) - return 1; - - if (isPowerOfTwo(_x)) - return _x; - - while (_x & (_x - 1)) - _x = _x & (_x - 1); - - _x = _x << 1; - - return _x; - } - - /// \brief Round a number up to the nearest multiple. For example, if - /// the input number is 12 and the multiple is 10, the result is 20. - /// If the input number is negative, then the nearest multiple will be - /// greater than or equal to the input number. For example, if the input - /// number is -9 and the multiple is 2 then the output is -8. - /// \param[in] _num Input number to round up. - /// \param[in] _multiple The multiple. If the multiple is <= zero, then - /// the input number is returned. - /// \return The nearest multiple of _multiple that is greater than - /// or equal to _num. - inline int roundUpMultiple(int _num, int _multiple) - { - if (_multiple == 0) - return _num; - - int remainder = std::abs(_num) % _multiple; - if (remainder == 0) - return _num; - - if (_num < 0) - return -(std::abs(_num) - remainder); - else - return _num + _multiple - remainder; - } - - /// \brief parse string into an integer - /// \param[in] _input the string - /// \return an integer, 0 or 0 and a message in the error stream - inline int parseInt(const std::string &_input) - { - // Return NAN_I if it is empty - if (_input.empty()) - { - return NAN_I; - } - // Return 0 if it is all spaces - else if (_input.find_first_not_of(' ') == std::string::npos) - { - return 0; - } - - // Otherwise try standard library - try - { - return std::stoi(_input); - } - // if that fails, return NAN_I - catch(...) - { - return NAN_I; - } - } - - /// \brief parse string into float - /// \param _input the string - /// \return a floating point number (can be NaN) or 0 with a message in the - /// error stream - inline double parseFloat(const std::string &_input) - { - // Return NAN_D if it is empty - if (_input.empty()) - { - return NAN_D; - } - // Return 0 if it is all spaces - else if (_input.find_first_not_of(' ') == std::string::npos) - { - return 0; - } - - // Otherwise try standard library - try - { - return std::stod(_input); - } - // if that fails, return NAN_D - catch(...) - { - return NAN_D; - } - } - - /// \brief Convert a std::chrono::steady_clock::time_point to a seconds and - /// nanoseconds pair. - /// \param[in] _time The time point to convert. - /// \return A pair where the first element is the number of seconds and - /// the second is the number of nanoseconds. - inline std::pair timePointToSecNsec( - const std::chrono::steady_clock::time_point &_time) - { - auto now_ns = std::chrono::duration_cast( - _time.time_since_epoch()); - auto now_s = std::chrono::duration_cast( - _time.time_since_epoch()); - int64_t seconds = now_s.count(); - int64_t nanoseconds = std::chrono::duration_cast - (now_ns - now_s).count(); - return {seconds, nanoseconds}; - } - - /// \brief Convert seconds and nanoseconds to - /// std::chrono::steady_clock::time_point. - /// \param[in] _sec The seconds to convert. - /// \param[in] _nanosec The nanoseconds to convert. - /// \return A std::chrono::steady_clock::time_point based on the number of - /// seconds and the number of nanoseconds. - inline std::chrono::steady_clock::time_point secNsecToTimePoint( - const uint64_t &_sec, const uint64_t &_nanosec) - { - auto duration = std::chrono::seconds(_sec) + std::chrono::nanoseconds( - _nanosec); - std::chrono::steady_clock::time_point result; - using std::chrono::duration_cast; - result += duration_cast(duration); - return result; - } - - /// \brief Convert seconds and nanoseconds to - /// std::chrono::steady_clock::duration. - /// \param[in] _sec The seconds to convert. - /// \param[in] _nanosec The nanoseconds to convert. - /// \return A std::chrono::steady_clock::duration based on the number of - /// seconds and the number of nanoseconds. - inline std::chrono::steady_clock::duration secNsecToDuration( - const uint64_t &_sec, const uint64_t &_nanosec) - { - return std::chrono::seconds(_sec) + std::chrono::nanoseconds( - _nanosec); - } - - /// \brief Convert a std::chrono::steady_clock::duration to a seconds and - /// nanoseconds pair. - /// \param[in] _dur The duration to convert. - /// \return A pair where the first element is the number of seconds and - /// the second is the number of nanoseconds. - inline std::pair durationToSecNsec( - const std::chrono::steady_clock::duration &_dur) - { - auto s = std::chrono::duration_cast(_dur); - auto ns = std::chrono::duration_cast(_dur-s); - return {s.count(), ns.count()}; - } - - // TODO(anyone): Replace this with std::chrono::days. - /// This will exist in C++-20 - typedef std::chrono::duration> days; - - /// \brief break down durations - /// NOTE: the template arguments must be properly ordered according - /// to magnitude and there can be no duplicates. - /// This function uses the braces initializer to split all the templated - /// duration. The initializer will be called recursievely due the `...` - /// \param[in] d Duration to break down - /// \return A tuple based on the durations specified - template - std::tuple breakDownDurations(DurationIn d) { - std::tuple retval; - using discard = int[]; - (void)discard{0, (void(( - (std::get(retval) = - std::chrono::duration_cast(d)), - (d -= std::chrono::duration_cast( - std::get(retval))))), 0)...}; - return retval; - } - - /// \brief Convert a std::chrono::steady_clock::time_point to a string - /// \param[in] _point The std::chrono::steady_clock::time_point to convert. - /// \return A string formatted with the time_point - inline std::string timePointToString( - const std::chrono::steady_clock::time_point &_point) - { - auto duration = _point - secNsecToTimePoint(0, 0); - auto cleanDuration = breakDownDurations( - duration); - std::ostringstream output_string; - output_string << std::setw(2) << std::setfill('0') - << std::get<0>(cleanDuration).count() << " " - << std::setw(2) << std::setfill('0') - << std::get<1>(cleanDuration).count() << ":" - << std::setw(2) << std::setfill('0') - << std::get<2>(cleanDuration).count() << ":" - << std::setfill('0') << std::setw(6) - << std::fixed << std::setprecision(3) - << std::get<3>(cleanDuration).count() + - std::get<4>(cleanDuration).count()/1000.0; - return output_string.str(); - } - - /// \brief Convert a std::chrono::steady_clock::duration to a string - /// \param[in] _duration The std::chrono::steady_clock::duration to convert. - /// \return A string formatted with the duration - inline std::string durationToString( - const std::chrono::steady_clock::duration &_duration) - { - auto cleanDuration = breakDownDurations( - _duration); - std::ostringstream outputString; - outputString << std::setw(2) << std::setfill('0') - << std::get<0>(cleanDuration).count() << " " - << std::setw(2) << std::setfill('0') - << std::get<1>(cleanDuration).count() << ":" - << std::setw(2) << std::setfill('0') - << std::get<2>(cleanDuration).count() << ":" - << std::setfill('0') << std::setw(6) - << std::fixed << std::setprecision(3) - << std::get<3>(cleanDuration).count() + - std::get<4>(cleanDuration).count()/1000.0; - return outputString.str(); - } - - // The following regex takes a time string in the general format of - // "dd hh:mm:ss.nnn" where n is milliseconds, if just one number is - // provided, it is assumed to be seconds - static const std::regex time_regex( - "^([0-9]+ ){0,1}" // day: - // Any positive integer - - "(?:([1-9]:|[0-1][0-9]:|2[0-3]:){0,1}" // hour: - // 1 - 9: - // 01 - 19: - // 20 - 23: - - "([0-9]:|[0-5][0-9]:)){0,1}" // minute: - // 0 - 9: - // 00 - 59: - - "(?:([0-9]|[0-5][0-9]){0,1}" // second: - // 0 - 9 - // 00 - 59 - - "(\\.[0-9]{1,3}){0,1})$"); // millisecond: - // .0 - .9 - // .00 - .99 - // .000 - 0.999 - - - /// \brief Check if the given string represents a time. - /// An example time string is "0 00:00:00.000", which has the format - /// "DAYS HOURS:MINUTES:SECONDS.MILLISECONDS" - /// \return True if the regex was able to split the string otherwise False - inline bool isTimeString(const std::string &_timeString) - { - std::smatch matches; - - // `matches` should always be a size of 6 as there are 6 matching - // groups in the regex. - // 1. The whole regex - // 2. The days - // 3. The hours - // 4. The minutes - // 5. The seconds - // 6. The milliseconds - // Note that the space will remain in the day match, the colon - // will remain in the hour and minute matches, and the period will - // remain in the millisecond match - if (!std::regex_search(_timeString, matches, time_regex) || - matches.size() != 6) - return false; - - std::string dayString = matches[1]; - - // Days are the only unbounded number, so check first to see if stoi - // runs successfully - if (!dayString.empty()) - { - // Erase the space - dayString.erase(dayString.length() - 1); - try - { - std::stoi(dayString); - } - catch (const std::out_of_range &) - { - return false; - } - } - - return true; - } - - /// \brief Split a std::chrono::steady_clock::duration to a string - /// \param[in] _timeString The string to convert in general format - /// \param[out] numberDays number of days in the string - /// \param[out] numberHours number of hours in the string - /// \param[out] numberMinutes number of minutes in the string - /// \param[out] numberSeconds number of seconds in the string - /// \param[out] numberMilliseconds number of milliseconds in the string - /// \return True if the regex was able to split the string otherwise False - inline bool splitTimeBasedOnTimeRegex( - const std::string &_timeString, - uint64_t & numberDays, uint64_t & numberHours, - uint64_t & numberMinutes, uint64_t & numberSeconds, - uint64_t & numberMilliseconds) - { - std::smatch matches; - - // `matches` should always be a size of 6 as there are 6 matching - // groups in the regex. - // 1. The whole regex - // 2. The days - // 3. The hours - // 4. The minutes - // 5. The seconds - // 6. The milliseconds - // We can also index them as such below. - // Note that the space will remain in the day match, the colon - // will remain in the hour and minute matches, and the period will - // remain in the millisecond match - if (!std::regex_search(_timeString, matches, time_regex) || - matches.size() != 6) - return false; - - std::string dayString = matches[1]; - std::string hourString = matches[2]; - std::string minuteString = matches[3]; - std::string secondString = matches[4]; - std::string millisecondString = matches[5]; - - // Days are the only unbounded number, so check first to see if stoi - // runs successfully - if (!dayString.empty()) - { - // Erase the space - dayString.erase(dayString.length() - 1); - try - { - numberDays = std::stoi(dayString); - } - catch (const std::out_of_range &) - { - return false; - } - } - - if (!hourString.empty()) - { - // Erase the colon - hourString.erase(hourString.length() - 1); - numberHours = std::stoi(hourString); - } - - if (!minuteString.empty()) - { - // Erase the colon - minuteString.erase(minuteString.length() - 1); - numberMinutes = std::stoi(minuteString); - } - - if (!secondString.empty()) - { - numberSeconds = std::stoi(secondString); - } - - if (!millisecondString.empty()) - { - // Erase the period - millisecondString.erase(0, 1); - - // Multiplier because "4" = 400 ms, "04" = 40 ms, and "004" = 4 ms - numberMilliseconds = std::stoi(millisecondString) * - static_cast(1000 / pow(10, millisecondString.length())); - } - return true; - } - - /// \brief Convert a string to a std::chrono::steady_clock::duration - /// \param[in] _timeString The string to convert in general format - /// "dd hh:mm:ss.nnn" where n is millisecond value - /// \return A std::chrono::steady_clock::duration containing the - /// string's time value. If it isn't possible to convert, the duration will - /// be zero. - inline std::chrono::steady_clock::duration stringToDuration( - const std::string &_timeString) - { - using namespace std::chrono_literals; - std::chrono::steady_clock::duration duration{ - std::chrono::steady_clock::duration::zero()}; - - if (_timeString.empty()) - return duration; - - uint64_t numberDays = 0; - uint64_t numberHours = 0; - uint64_t numberMinutes = 0; - uint64_t numberSeconds = 0; - uint64_t numberMilliseconds = 0; - - if (!splitTimeBasedOnTimeRegex(_timeString, numberDays, numberHours, - numberMinutes, numberSeconds, - numberMilliseconds)) - { - return duration; - } - - // TODO(anyone): Replace below day conversion with std::chrono::days. - /// This will exist in C++-20 - duration = std::chrono::steady_clock::duration::zero(); - auto delta = std::chrono::milliseconds(numberMilliseconds) + - std::chrono::seconds(numberSeconds) + - std::chrono::minutes(numberMinutes) + - std::chrono::hours(numberHours) + - std::chrono::hours(24 * numberDays); - duration += delta; - - return duration; - } - - /// \brief Convert a string to a std::chrono::steady_clock::time_point - /// \param[in] _timeString The string to convert in general format - /// "dd hh:mm:ss.nnn" where n is millisecond value - /// \return A std::chrono::steady_clock::time_point containing the - /// string's time value. If it isn't possible to convert, the time will - /// be negative 1 second. - inline std::chrono::steady_clock::time_point stringToTimePoint( - const std::string &_timeString) - { - using namespace std::chrono_literals; - std::chrono::steady_clock::time_point timePoint{-1s}; - - if (_timeString.empty()) - return timePoint; - - uint64_t numberDays = 0; - uint64_t numberHours = 0; - uint64_t numberMinutes = 0; - uint64_t numberSeconds = 0; - uint64_t numberMilliseconds = 0; - - if (!splitTimeBasedOnTimeRegex(_timeString, numberDays, numberHours, - numberMinutes, numberSeconds, - numberMilliseconds)) - { - return timePoint; - } - - // TODO(anyone): Replace below day conversion with std::chrono::days. - /// This will exist in C++-20 - timePoint = math::secNsecToTimePoint(0, 0); - auto duration = std::chrono::milliseconds(numberMilliseconds) + - std::chrono::seconds(numberSeconds) + - std::chrono::minutes(numberMinutes) + - std::chrono::hours(numberHours) + - std::chrono::hours(24 * numberDays); - timePoint += duration; - - return timePoint; - } - - // Degrade precision on Windows, which cannot handle 'long double' - // values properly. See the implementation of Unpair. - // 32 bit ARM processors also define 'long double' to be the same - // size as 'double', and must also be degraded -#if defined _MSC_VER || defined __arm__ - using PairInput = uint16_t; - using PairOutput = uint32_t; -#else - using PairInput = uint32_t; - using PairOutput = uint64_t; -#endif - - /// \brief A pairing function that maps two values to a unique third - /// value. This is an implement of Szudzik's function. - /// \param[in] _a First value, must be a non-negative integer. On - /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t. - /// \param[in] _b Second value, must be a non-negative integer. On - /// Windows this value is uint16_t. On Linux/OSX this value is uint32_t. - /// \return A unique non-negative integer value. On Windows the return - /// value is uint32_t. On Linux/OSX the return value is uint64_t - /// \sa Unpair - PairOutput IGNITION_MATH_VISIBLE Pair( - const PairInput _a, const PairInput _b); - - /// \brief The reverse of the Pair function. Accepts a key, produced - /// from the Pair function, and returns a tuple consisting of the two - /// non-negative integer values used to create the _key. - /// \param[in] _key A non-negative integer generated from the Pair - /// function. On Windows this value is uint32_t. On Linux/OSX, this - /// value is uint64_t. - /// \return A tuple that consists of the two non-negative integers that - /// will generate _key when used with the Pair function. On Windows the - /// tuple contains two uint16_t values. On Linux/OSX the tuple contains - /// two uint32_t values. - /// \sa Pair - std::tuple IGNITION_MATH_VISIBLE Unpair( - const PairOutput _key); - } - } -} - -#endif diff --git a/include/ignition/math/Inertial.hh b/include/ignition/math/Inertial.hh index 91f486262..2f2b93e75 100644 --- a/include/ignition/math/Inertial.hh +++ b/include/ignition/math/Inertial.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,272 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_INERTIAL_HH_ -#define IGNITION_MATH_INERTIAL_HH_ + */ +#include #include -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Pose3.hh" - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Inertial Inertial.hh ignition/math/Inertial.hh - /// \brief The Inertial object provides a representation for the mass and - /// inertia matrix of a body B. The components of the inertia matrix are - /// expressed in what we call the "inertial" frame Bi of the body, i.e. - /// the frame in which these inertia components are measured. The inertial - /// frame Bi must be located at the center of mass of the body, but not - /// necessarily aligned with the body’s frame. In addition, this class - /// allows users to specify a frame F for these inertial properties by - /// specifying the pose X_FBi of the inertial frame Bi in the - /// inertial object frame F. - /// - /// For information about the X_FBi notation, see - /// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html - template - class Inertial - { - /// \brief Default Constructor - public: Inertial() - {} - - /// \brief Constructs an inertial object from the mass matrix for a body - /// B, about its center of mass Bcm, and expressed in a frame that we’ll - /// call the "inertial" frame Bi, i.e. the frame in which the components - /// of the mass matrix are specified (see this class’s documentation for - /// details). The pose object specifies the pose X_FBi of the inertial - /// frame Bi in the frame F of this inertial object - /// (see class’s documentation). - /// \param[in] _massMatrix Mass and inertia matrix. - /// \param[in] _pose Pose of center of mass reference frame. - public: Inertial(const MassMatrix3 &_massMatrix, - const Pose3 &_pose) - : massMatrix(_massMatrix), pose(_pose) - {} - - /// \brief Copy constructor. - /// \param[in] _inertial Inertial element to copy - public: Inertial(const Inertial &_inertial) - : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose()) - {} - - /// \brief Destructor. - public: virtual ~Inertial() {} - - /// \brief Set the mass and inertia matrix. - /// - /// \param[in] _m New MassMatrix3 object. - /// \param[in] _tolerance Tolerance is passed to - /// MassMatrix3::IsValid and is the amount of error - /// to accept when checking whether the MassMatrix3 _m is valid. - /// Refer to MassMatrix3::Epsilon for detailed description of - /// _tolerance. - /// - /// \return True if the MassMatrix3 is valid. - public: bool SetMassMatrix(const MassMatrix3 &_m, - const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE) - { - this->massMatrix = _m; - return this->massMatrix.IsValid(_tolerance); - } - - /// \brief Get the mass and inertia matrix. - /// \return The mass matrix about the body’s center of mass and - /// expressed in the inertial frame Bi as defined by this class’s - /// documentation - public: const MassMatrix3 &MassMatrix() const - { - return this->massMatrix; - } - - /// \brief Set the pose of the center of mass reference frame. - /// \param[in] _pose New pose. - /// \return True if the MassMatrix3 is valid. - public: bool SetPose(const Pose3 &_pose) - { - this->pose = _pose; - return this->massMatrix.IsValid(); - } - - /// \brief Get the pose of the center of mass reference frame. - /// \return The pose of the inertial frame Bi in the frame F of this - /// Inertial object as defined by this class’s documentation. - public: const Pose3 &Pose() const - { - return this->pose; - } - - /// \copydoc Moi() const - /// \deprecated See Matrix3 Moi() const - public: Matrix3 IGN_DEPRECATED(5.0) MOI() const - { - return this->Moi(); - } - - /// \brief Get the moment of inertia matrix computer about the body's - /// center of mass and expressed in this Inertial object’s frame F. - /// \return The inertia matrix computed about the body’s center of - /// mass and expressed in this Inertial object’s frame F, as defined - /// in this class’s documentation. - public: Matrix3 Moi() const - { - auto R = Matrix3(this->pose.Rot()); - return R * this->massMatrix.Moi() * R.Transposed(); - } - - /// \brief Set the inertial pose rotation without affecting the - /// MOI in the base coordinate frame. - /// \param[in] _q New rotation for inertial pose. - /// \return True if the MassMatrix3 is valid. - public: bool SetInertialRotation(const Quaternion &_q) - { - auto moi = this->Moi(); - this->pose.Rot() = _q; - auto R = Matrix3(_q); - return this->massMatrix.SetMoi(R.Transposed() * moi * R); - } - - /// \brief Set the MassMatrix rotation (eigenvectors of inertia matrix) - /// without affecting the MOI in the base coordinate frame. - /// Note that symmetries in inertia matrix may prevent the output of - /// MassMatrix3::PrincipalAxesOffset to match this function's input _q, - /// but it is guaranteed that the MOI in the base frame will not change. - /// A negative value of _tol (such as -1e-6) can be passed to ensure - /// that diagonal values are always sorted. - /// \param[in] _q New rotation. - /// \param[in] _tol Relative tolerance given by absolute value - /// of _tol. This is passed to the MassMatrix3 - /// PrincipalMoments and PrincipalAxesOffset functions. - /// \return True if the MassMatrix3 is valid. - public: bool SetMassMatrixRotation(const Quaternion &_q, - const T _tol = 1e-6) - { - this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) * - _q.Inverse(); - const auto moments = this->MassMatrix().PrincipalMoments(_tol); - const auto diag = Matrix3( - moments[0], 0, 0, - 0, moments[1], 0, - 0, 0, moments[2]); - const auto R = Matrix3(_q); - return this->massMatrix.SetMoi(R * diag * R.Transposed()); - } - - /// \brief Equal operator. - /// \param[in] _inertial Inertial to copy. - /// \return Reference to this object. - public: Inertial &operator=(const Inertial &_inertial) - { - this->massMatrix = _inertial.MassMatrix(); - this->pose = _inertial.Pose(); - - return *this; - } - - /// \brief Equality comparison operator. - /// \param[in] _inertial Inertial to copy. - /// \return true if each component is equal within a default tolerance, - /// false otherwise - public: bool operator==(const Inertial &_inertial) const - { - return (this->pose == _inertial.Pose()) && - (this->massMatrix == _inertial.MassMatrix()); - } - - /// \brief Inequality test operator - /// \param[in] _inertial Inertial to test - /// \return True if not equal (using the default tolerance of 1e-6) - public: bool operator!=(const Inertial &_inertial) const - { - return !(*this == _inertial); - } - - /// \brief Adds inertial properties to current object. - /// The mass, center of mass location, and inertia matrix are updated - /// as long as the total mass is positive. - /// \param[in] _inertial Inertial to add. - /// \return Reference to this object. - public: Inertial &operator+=(const Inertial &_inertial) - { - T m1 = this->massMatrix.Mass(); - T m2 = _inertial.MassMatrix().Mass(); - - // Total mass - T mass = m1 + m2; - - // Only continue if total mass is positive - if (mass <= 0) - { - return *this; - } - - auto com1 = this->Pose().Pos(); - auto com2 = _inertial.Pose().Pos(); - // New center of mass location in base frame - auto com = (m1*com1 + m2*com2) / mass; - - // Components of new moment of inertia matrix - Vector3 ixxyyzz; - Vector3 ixyxzyz; - // First add matrices in base frame - { - auto moi = this->Moi() + _inertial.Moi(); - ixxyyzz = Vector3(moi(0, 0), moi(1, 1), moi(2, 2)); - ixyxzyz = Vector3(moi(0, 1), moi(0, 2), moi(1, 2)); - } - // Then account for parallel axis theorem - { - auto dc = com1 - com; - ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2)); - ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2)); - ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2)); - ixyxzyz.X() -= m1 * dc[0] * dc[1]; - ixyxzyz.Y() -= m1 * dc[0] * dc[2]; - ixyxzyz.Z() -= m1 * dc[1] * dc[2]; - } - { - auto dc = com2 - com; - ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2)); - ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2)); - ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2)); - ixyxzyz.X() -= m2 * dc[0] * dc[1]; - ixyxzyz.Y() -= m2 * dc[0] * dc[2]; - ixyxzyz.Z() -= m2 * dc[1] * dc[2]; - } - this->massMatrix = MassMatrix3(mass, ixxyyzz, ixyxzyz); - this->pose = Pose3(com, Quaternion::Identity); - - return *this; - } - - /// \brief Adds inertial properties to current object. - /// The mass, center of mass location, and inertia matrix are updated - /// as long as the total mass is positive. - /// \param[in] _inertial Inertial to add. - /// \return Sum of inertials as new object. - public: const Inertial operator+(const Inertial &_inertial) const - { - return Inertial(*this) += _inertial; - } - - /// \brief Mass and inertia matrix of the object expressed in the - /// center of mass reference frame. - private: MassMatrix3 massMatrix; - - /// \brief Pose offset of center of mass reference frame relative - /// to a base frame. - private: Pose3 pose; - }; - - typedef Inertial Inertiald; - typedef Inertial Inertialf; - } - } -} -#endif diff --git a/include/ignition/math/Interval.hh b/include/ignition/math/Interval.hh index 115db95dc..ce17d0916 100644 --- a/include/ignition/math/Interval.hh +++ b/include/ignition/math/Interval.hh @@ -13,287 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_INTERVAL_HH_ -#define IGNITION_MATH_INTERVAL_HH_ - -#include -#include -#include -#include -#include + */ +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Interval Interval.hh ignition/math/Interval.hh - /// \brief The Interval class represents a range of real numbers. - /// Intervals may be open (a, b), left-closed [a, b), right-closed - /// (a, b], or fully closed [a, b]. - /// - /// ## Example - /// - /// \snippet examples/interval_example.cc complete - template - class Interval - { - /// \brief An unbounded interval (-∞, ∞) - public: static const Interval &Unbounded; - - /// \brief Constructor - public: Interval() = default; - - /// \brief Constructor - /// \param[in] _leftValue leftmost interval value - /// \param[in] _leftClosed whether the interval is - /// left-closed or not - /// \param[in] _rightValue rightmost interval value - /// \param[in] _rightClosed whether the interval - /// is right-closed or not - public: constexpr Interval( - T _leftValue, bool _leftClosed, - T _rightValue, bool _rightClosed) - : leftValue(std::move(_leftValue)), - rightValue(std::move(_rightValue)), - leftClosed(_leftClosed), - rightClosed(_rightClosed) - { - } - - /// \brief Make an open interval (`_leftValue`, `_rightValue`) - /// \param[in] _leftValue leftmost interval value - /// \param[in] _rightValue rightmost interval value - /// \return the open interval - public: static constexpr Interval - Open(T _leftValue, T _rightValue) - { - return Interval( - std::move(_leftValue), false, - std::move(_rightValue), false); - } - - /// \brief Make a left-closed interval [`_leftValue`, `_rightValue`) - /// \param[in] _leftValue leftmost interval value - /// \param[in] _rightValue rightmost interval value - /// \return the left-closed interval - public: static constexpr Interval - LeftClosed(T _leftValue, T _rightValue) - { - return Interval( - std::move(_leftValue), true, - std::move(_rightValue), false); - } - - /// \brief Make a right-closed interval (`_leftValue`, `_rightValue`] - /// \param[in] _leftValue leftmost interval value - /// \param[in] _rightValue rightmost interval value - /// \return the left-closed interval - public: static constexpr Interval - RightClosed(T _leftValue, T _rightValue) - { - return Interval( - std::move(_leftValue), false, - std::move(_rightValue), true); - } - - /// \brief Make a closed interval [`_leftValue`, `_rightValue`] - /// \param[in] _leftValue leftmost interval value - /// \param[in] _rightValue rightmost interval value - /// \return the closed interval - public: static constexpr Interval - Closed(T _leftValue, T _rightValue) - { - return Interval{ - std::move(_leftValue), true, - std::move(_rightValue), true}; - } - - /// \brief Get the leftmost interval value - /// \return the leftmost interval value - public: const T &LeftValue() const { return this->leftValue; } - - /// \brief Check if the interval is left-closed - /// \return true if the interval is left-closed, false otherwise - public: bool IsLeftClosed() const { return this->leftClosed; } - - /// \brief Get the rightmost interval value - /// \return the rightmost interval value - public: const T &RightValue() const { return this->rightValue; } - - /// \brief Check if the interval is right-closed - /// \return true if the interval is right-closed, false otherwise - public: bool IsRightClosed() const { return this->rightClosed; } - - /// \brief Check if the interval is empty - /// Some examples of empty intervals include - /// (a, a), [a, a), and [a + 1, a]. - /// \return true if it is empty, false otherwise - public: bool Empty() const - { - if (this->leftClosed && this->rightClosed) - { - return this->rightValue < this->leftValue; - } - return this->rightValue <= this->leftValue; - } - - /// \brief Check if the interval contains `_value` - /// \param[in] _value value to check for membership - /// \return true if it is contained, false otherwise - public: bool Contains(const T &_value) const - { - if (this->leftClosed && this->rightClosed) - { - return this->leftValue <= _value && _value <= this->rightValue; - } - if (this->leftClosed) - { - return this->leftValue <= _value && _value < this->rightValue; - } - if (this->rightClosed) - { - return this->leftValue < _value && _value <= this->rightValue; - } - return this->leftValue < _value && _value < this->rightValue; - } - - /// \brief Check if the interval contains `_other` interval - /// \param[in] _other interval to check for membership - /// \return true if it is contained, false otherwise - public: bool Contains(const Interval &_other) const - { - if (this->Empty() || _other.Empty()) - { - return false; - } - if (!this->leftClosed && _other.leftClosed) - { - if (_other.leftValue <= this->leftValue) - { - return false; - } - } - else - { - if (_other.leftValue < this->leftValue) - { - return false; - } - } - if (!this->rightClosed && _other.rightClosed) - { - if (this->rightValue <= _other.rightValue) - { - return false; - } - } - else - { - if (this->rightValue < _other.rightValue) - { - return false; - } - } - return true; - } - - /// \brief Check if the interval intersects `_other` interval - /// \param[in] _other interval to check for intersection - /// \return true if both intervals intersect, false otherwise - public: bool Intersects(const Interval &_other) const - { - if (this->Empty() || _other.Empty()) - { - return false; - } - if (this->rightClosed && _other.leftClosed) - { - if (this->rightValue < _other.leftValue) - { - return false; - } - } - else - { - if (this->rightValue <= _other.leftValue) - { - return false; - } - } - if (_other.rightClosed && this->leftClosed) - { - if (_other.rightValue < this->leftValue) - { - return false; - } - } - else - { - if (_other.rightValue <= this->leftValue) - { - return false; - } - } - return true; - } - - /// \brief Equality test operator - /// \param _other interval to check for equality - /// \return true if intervals are equal, false otherwise - public: bool operator==(const Interval &_other) const - { - return this->Contains(_other) && _other.Contains(*this); - } - - /// \brief Inequality test operator - /// \param _other interval to check for inequality - /// \return true if intervals are unequal, false otherwise - public: bool operator!=(const Interval &_other) const - { - return !this->Contains(_other) || !_other.Contains(*this); - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _interval Interval to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Interval &_interval) - { - return _out << (_interval.leftClosed ? "[" : "(") - << _interval.leftValue << ", " << _interval.rightValue - << (_interval.rightClosed ? "]" : ")"); - } - - /// \brief The leftmost interval value - private: T leftValue{0}; - /// \brief The righmost interval value - private: T rightValue{0}; - /// \brief Whether the interval is left-closed or not - private: bool leftClosed{false}; - /// \brief Whether the interval is right-closed or not - private: bool rightClosed{false}; - }; - - namespace detail { - template - constexpr Interval gUnboundedInterval = - Interval::Open(-std::numeric_limits::infinity(), - std::numeric_limits::infinity()); - } // namespace detail - template - const Interval &Interval::Unbounded = detail::gUnboundedInterval; - - using Intervalf = Interval; - using Intervald = Interval; - } - } -} - -#endif diff --git a/include/ignition/math/Kmeans.hh b/include/ignition/math/Kmeans.hh index 43d4231d4..7067b32e3 100644 --- a/include/ignition/math/Kmeans.hh +++ b/include/ignition/math/Kmeans.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Open Source Robotics Foundation + * 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. @@ -13,78 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_KMEANS_HH_ -#define IGNITION_MATH_KMEANS_HH_ + */ -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declare private data - class KmeansPrivate; - - /// \class Kmeans Kmeans.hh math/gzmath.hh - /// \brief K-Means clustering algorithm. Given a set of observations, - /// k-means partitions the observations into k sets so as to minimize the - /// within-cluster sum of squares. - /// Description based on http://en.wikipedia.org/wiki/K-means_clustering. - class IGNITION_MATH_VISIBLE Kmeans - { - /// \brief constructor - /// \param[in] _obs Set of observations to cluster. - public: explicit Kmeans(const std::vector &_obs); - - /// \brief Destructor. - public: virtual ~Kmeans(); - - /// \brief Get the observations to cluster. - /// \return The vector of observations. - public: std::vector Observations() const; - - /// \brief Set the observations to cluster. - /// \param[in] _obs The new vector of observations. - /// \return True if the vector is not empty or false otherwise. - public: bool Observations(const std::vector &_obs); - - /// \brief Add observations to the cluster. - /// \param[in] _obs Vector of observations. - /// \return True if the _obs vector is not empty or false otherwise. - public: bool AppendObservations(const std::vector &_obs); - - /// \brief Executes the k-means algorithm. - /// \param[in] _k Number of partitions to cluster. - /// \param[out] _centroids Vector of centroids. Each element contains the - /// centroid of one cluster. - /// \param[out] _labels Vector of labels. The size of this vector is - /// equals to the number of observations. Each element represents the - /// cluster to which observation belongs. - /// \return True when the operation succeed or false otherwise. The - /// operation will fail if the number of observations is not positive, - /// if the number of clusters is non positive, or if the number of - /// clusters if greater than the number of observations. - public: bool Cluster(int _k, - std::vector &_centroids, - std::vector &_labels); - - /// \brief Given an observation, it returns the closest centroid to it. - /// \param[in] _p Point to check. - /// \return The index of the closest centroid to the point _p. - private: unsigned int ClosestCentroid(const Vector3d &_p) const; - - /// \brief Private data pointer - private: KmeansPrivate *dataPtr; - }; - } - } -} - -#endif diff --git a/include/ignition/math/Line2.hh b/include/ignition/math/Line2.hh index 0b669f868..bbdefa9ea 100644 --- a/include/ignition/math/Line2.hh +++ b/include/ignition/math/Line2.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Open Source Robotics Foundation + * 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. @@ -13,308 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_LINE2_HH_ -#define IGNITION_MATH_LINE2_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Line2 Line2.hh ignition/math/Line2.hh - /// \brief A two dimensional line segment. The line is defined by a - /// start and end point. - template - class Line2 - { - /// \brief Constructor. - /// \param[in] _ptA Start point of the line segment - /// \param[in] _ptB End point of the line segment - public: Line2(const math::Vector2 &_ptA, const math::Vector2 &_ptB) - { - this->Set(_ptA, _ptB); - } - - /// \brief Constructor. - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - public: Line2(double _x1, double _y1, double _x2, double _y2) - { - this->Set(_x1, _y1, _x2, _y2); - } - - /// \brief Set the start and end point of the line segment - /// \param[in] _ptA Start point of the line segment - /// \param[in] _ptB End point of the line segment - public: void Set(const math::Vector2 &_ptA, - const math::Vector2 &_ptB) - { - this->pts[0] = _ptA; - this->pts[1] = _ptB; - } - - /// \brief Set the start and end point of the line segment - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - public: void Set(double _x1, double _y1, double _x2, double _y2) - { - this->pts[0].Set(_x1, _y1); - this->pts[1].Set(_x2, _y2); - } - - /// \brief Return the cross product of this line and the given line. - /// Give 'a' as this line and 'b' as given line, the equation is: - /// (a.start.x - a.end.x) * (b.start.y - b.end.y) - - /// (a.start.y - a.end.y) * (b.start.x - b.end.x) - /// \param[in] _line Line for the cross product computation. - /// \return Return the cross product of this line and the given line. - public: double CrossProduct(const Line2 &_line) const - { - return (this->pts[0].X() - this->pts[1].X()) * - (_line[0].Y() -_line[1].Y()) - - (this->pts[0].Y() - this->pts[1].Y()) * - (_line[0].X() - _line[1].X()); - } - - /// \brief Return the cross product of this line and the given point. - /// Given 'a' and 'b' as the start and end points, the equation is: - // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y) - /// \param[in] _pt Point for the cross product computation. - /// \return Return the cross product of this line and the given point. - public: double CrossProduct(const Vector2 &_pt) const - { - return (_pt.Y() - this->pts[0].Y()) * - (this->pts[1].X() - this->pts[0].X()) - - (_pt.X() - this->pts[0].X()) * - (this->pts[1].Y() - this->pts[0].Y()); - } - - /// \brief Check if the given point is collinear with this line. - /// \param[in] _pt The point to check. - /// \param[in] _epsilon The error bounds within which the collinear - /// check will return true. - /// \return Return true if the point is collinear with this line, false - /// otherwise. - public: bool Collinear(const math::Vector2 &_pt, - double _epsilon = 1e-6) const - { - return math::equal(this->CrossProduct(_pt), - 0., _epsilon); - } - - /// \brief Check if the given line is parallel with this line. - /// \param[in] _line The line to check. - /// \param[in] _epsilon The error bounds within which the parallel - /// check will return true. - /// \return Return true if the line is parallel with this line, false - /// otherwise. Return true if either line is a point (line with zero - /// length). - public: bool Parallel(const math::Line2 &_line, - double _epsilon = 1e-6) const - { - return math::equal(this->CrossProduct(_line), - 0., _epsilon); - } - - /// \brief Check if the given line is collinear with this line. This - /// is the AND of Parallel and Intersect. - /// \param[in] _line The line to check. - /// \param[in] _epsilon The error bounds within which the collinear - /// check will return true. - /// \return Return true if the line is collinear with this line, false - /// otherwise. - public: bool Collinear(const math::Line2 &_line, - double _epsilon = 1e-6) const - { - return this->Parallel(_line, _epsilon) && - this->Intersect(_line, _epsilon); - } - - /// \brief Return whether the given point is on this line segment. - /// \param[in] _pt Point to check. - /// \param[in] _epsilon The error bounds within which the OnSegment - /// check will return true. - /// \return True if the point is on the segement. - public: bool OnSegment(const math::Vector2 &_pt, - double _epsilon = 1e-6) const - { - return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon); - } - - /// \brief Check if the given point is between the start and end - /// points of the line segment. This does not imply that the point is - /// on the segment. - /// \param[in] _pt Point to check. - /// \param[in] _epsilon The error bounds within which the within - /// check will return true. - /// \return True if the point is on the segement. - public: bool Within(const math::Vector2 &_pt, - double _epsilon = 1e-6) const - { - return _pt.X() <= std::max(this->pts[0].X(), - this->pts[1].X()) + _epsilon && - _pt.X() >= std::min(this->pts[0].X(), - this->pts[1].X()) - _epsilon && - _pt.Y() <= std::max(this->pts[0].Y(), - this->pts[1].Y()) + _epsilon && - _pt.Y() >= std::min(this->pts[0].Y(), - this->pts[1].Y()) - _epsilon; - } - - /// \brief Check if this line intersects the given line segment. - /// \param[in] _line The line to check for intersection. - /// \param[in] _epsilon The error bounds within which the intersection - /// check will return true. - /// \return True if an intersection was found. - public: bool Intersect(const Line2 &_line, - double _epsilon = 1e-6) const - { - static math::Vector2 ignore; - return this->Intersect(_line, ignore, _epsilon); - } - - /// \brief Check if this line intersects the given line segment. The - /// point of intersection is returned in the _result parameter. - /// \param[in] _line The line to check for intersection. - /// \param[out] _pt The point of intersection. This value is only - /// valid if the return value is true. - /// \param[in] _epsilon The error bounds within which the intersection - /// check will return true. - /// \return True if an intersection was found. - public: bool Intersect(const Line2 &_line, math::Vector2 &_pt, - double _epsilon = 1e-6) const - { - double d = this->CrossProduct(_line); - - // d is zero if the two line are collinear. Must check special - // cases. - if (math::equal(d, 0.0, _epsilon)) - { - // Check if _line's starting point is on the line. - if (this->Within(_line[0], _epsilon)) - { - _pt = _line[0]; - return true; - } - // Check if _line's ending point is on the line. - else if (this->Within(_line[1], _epsilon)) - { - _pt = _line[1]; - return true; - } - // Other wise return false. - else - return false; - } - - _pt.X((_line[0].X() - _line[1].X()) * - (this->pts[0].X() * this->pts[1].Y() - - this->pts[0].Y() * this->pts[1].X()) - - (this->pts[0].X() - this->pts[1].X()) * - (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X())); - - _pt.Y((_line[0].Y() - _line[1].Y()) * - (this->pts[0].X() * this->pts[1].Y() - - this->pts[0].Y() * this->pts[1].X()) - - (this->pts[0].Y() - this->pts[1].Y()) * - (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X())); - - _pt /= d; - - if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) || - _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) || - _pt.X() < std::min(_line[0].X(), _line[1].X()) || - _pt.X() > std::max(_line[0].X(), _line[1].X())) - { - return false; - } - - if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) || - _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) || - _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) || - _pt.Y() > std::max(_line[0].Y(), _line[1].Y())) - { - return false; - } - - return true; - } - - /// \brief Get the length of the line - /// \return The length of the line. - public: T Length() const - { - return sqrt((this->pts[0].X() - this->pts[1].X()) * - (this->pts[0].X() - this->pts[1].X()) + - (this->pts[0].Y() - this->pts[1].Y()) * - (this->pts[0].Y() - this->pts[1].Y())); - } - - /// \brief Get the slope of the line - /// \return The slope of the line, NAN_D if the line is vertical. - public: double Slope() const - { - if (math::equal(this->pts[1].X(), this->pts[0].X())) - return NAN_D; - - return (this->pts[1].Y() - this->pts[0].Y()) / - static_cast(this->pts[1].X() - this->pts[0].X()); - } - - /// \brief Equality operator. - /// \param[in] _line Line to compare for equality. - /// \return True if the given line is equal to this line - public: bool operator==(const Line2 &_line) const - { - return this->pts[0] == _line[0] && this->pts[1] == _line[1]; - } - - /// \brief Inequality operator. - /// \param[in] _line Line to compare for inequality. - /// \return True if the given line is not to this line - public: bool operator!=(const Line2 &_line) const - { - return !(*this == _line); - } - - /// \brief Get the start or end point. - /// \param[in] _index 0 = start point, 1 = end point. The _index - /// is clamped to the range [0, 1] - public: math::Vector2 operator[](size_t _index) const - { - return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; - } - - /// \brief Stream extraction operator - /// \param[in] _out output stream - /// \param[in] _line Line2 to output - /// \return The stream - public: friend std::ostream &operator<<( - std::ostream &_out, const Line2 &_line) - { - _out << _line[0] << " " << _line[1]; - return _out; - } - - private: math::Vector2 pts[2]; - }; - - - typedef Line2 Line2i; - typedef Line2 Line2d; - typedef Line2 Line2f; - } - } -} -#endif diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index e927817b8..23795634a 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Open Source Robotics Foundation + * 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. @@ -13,412 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_LINE3_HH_ -#define IGNITION_MATH_LINE3_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Line3 Line3.hh ignition/math/Line3.hh - /// \brief A three dimensional line segment. The line is defined by a - /// start and end point. - template - class Line3 - { - /// \brief Line Constructor - public: Line3() = default; - - /// \brief Copy constructor - /// \param[in] _line a line object - public: Line3(const Line3 &_line) - { - this->pts[0] = _line[0]; - this->pts[1] = _line[1]; - } - - /// \brief Constructor. - /// \param[in] _ptA Start point of the line segment - /// \param[in] _ptB End point of the line segment - public: Line3(const math::Vector3 &_ptA, const math::Vector3 &_ptB) - { - this->Set(_ptA, _ptB); - } - - /// \brief 2D Constructor where Z coordinates are 0 - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - public: Line3(const double _x1, const double _y1, - const double _x2, const double _y2) - { - this->Set(_x1, _y1, _x2, _y2); - } - - /// \brief Constructor. - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _z1 Z coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - /// \param[in] _z2 Z coordinate of the end point. - public: Line3(const double _x1, const double _y1, - const double _z1, const double _x2, - const double _y2, const double _z2) - { - this->Set(_x1, _y1, _z1, _x2, _y2, _z2); - } - - /// \brief Set the start and end point of the line segment - /// \param[in] _ptA Start point of the line segment - /// \param[in] _ptB End point of the line segment - public: void Set(const math::Vector3 &_ptA, - const math::Vector3 &_ptB) - { - this->pts[0] = _ptA; - this->pts[1] = _ptB; - } - - /// \brief Set the start point of the line segment - /// \param[in] _ptA Start point of the line segment - public: void SetA(const math::Vector3 &_ptA) - { - this->pts[0] = _ptA; - } - - /// \brief Set the end point of the line segment - /// \param[in] _ptB End point of the line segment - public: void SetB(const math::Vector3 &_ptB) - { - this->pts[1] = _ptB; - } - - /// \brief Set the start and end point of the line segment, assuming that - /// both points have the same height. - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - /// \param[in] _z Z coordinate of both points, - /// by default _z is set to 0. - public: void Set(const double _x1, const double _y1, - const double _x2, const double _y2, - const double _z = 0) - { - this->pts[0].Set(_x1, _y1, _z); - this->pts[1].Set(_x2, _y2, _z); - } - - /// \brief Set the start and end point of the line segment - /// \param[in] _x1 X coordinate of the start point. - /// \param[in] _y1 Y coordinate of the start point. - /// \param[in] _z1 Z coordinate of the start point. - /// \param[in] _x2 X coordinate of the end point. - /// \param[in] _y2 Y coordinate of the end point. - /// \param[in] _z2 Z coordinate of the end point. - public: void Set(const double _x1, const double _y1, - const double _z1, const double _x2, - const double _y2, const double _z2) - { - this->pts[0].Set(_x1, _y1, _z1); - this->pts[1].Set(_x2, _y2, _z2); - } - - /// \brief Get the direction of the line - /// \return The direction vector - public: math::Vector3 Direction() const - { - return (this->pts[1] - this->pts[0]).Normalize(); - } - - /// \brief Get the length of the line - /// \return The length of the line. - public: T Length() const - { - return this->pts[0].Distance(this->pts[1]); - } - - /// \brief Get the shortest line between this line and the - /// provided line. - /// - /// In the case when the two lines are parallel, we choose the first - /// point of this line and the closest point in the provided line. - /// \param[in] _line Line to compare against this. - /// \param[out] _result The shortest line between _line and this. - /// \param[in] _epsilon Error tolerance. - /// \return True if a solution was found. False if a solution is not - /// possible. - public: bool Distance(const Line3 &_line, Line3 &_result, - const double _epsilon = 1e-6) const - { - Vector3 p13 = this->pts[0] - _line[0]; - Vector3 p43 = _line[1] - _line[0]; - - if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon && - std::abs(p43.Z()) < _epsilon) - { - return false; - } - - Vector3 p21 = this->pts[1] - this->pts[0]; - - if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon && - std::abs(p21.Z()) < _epsilon) - { - return false; - } - - double d1343 = p13.Dot(p43); - double d4321 = p43.Dot(p21); - double d1321 = p13.Dot(p21); - double d4343 = p43.Dot(p43); - double d2121 = p21.Dot(p21); - - double denom = d2121 * d4343 - d4321 * d4321; - - // In this case, we choose the first point in this line, - // and the closest point in the provided line. - if (std::abs(denom) < _epsilon) - { - double d1 = this->pts[0].Distance(_line[0]); - double d2 = this->pts[0].Distance(_line[1]); - - double d3 = this->pts[1].Distance(_line[0]); - double d4 = this->pts[1].Distance(_line[1]); - - if (d1 <= d2 && d1 <= d3 && d1 <= d4) - { - _result.SetA(this->pts[0]); - _result.SetB(_line[0]); - } - else if (d2 <= d3 && d2 <= d4) - { - _result.SetA(this->pts[0]); - _result.SetB(_line[1]); - } - else if (d3 <= d4) - { - _result.SetA(this->pts[1]); - _result.SetB(_line[0]); - } - else - { - _result.SetA(this->pts[1]); - _result.SetB(_line[1]); - } - - return true; - } - - double numer = d1343 * d4321 - d1321 * d4343; - - double mua = clamp(numer / denom, 0.0, 1.0); - double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0); - - _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub)); - - return true; - } - - /// \brief Calculate shortest distance between line and point - /// \param[in] _pt Point which we are measuring distance to. - /// \returns Distance from point to line. - public: T Distance(const Vector3 &_pt) - { - auto line = this->pts[1] - this->pts[0]; - auto ptTo0 = _pt - this->pts[0]; - auto ptTo1 = _pt - this->pts[1]; - - // Point is projected beyond pt0 or the line has length 0 - if (ptTo0.Dot(line) <= 0.0) - { - return ptTo0.Length(); - } - - // Point is projected beyond pt1 - if (ptTo1.Dot(line) >= 0.0) - { - return ptTo1.Length(); - } - - // Distance to point projected onto line - // line.Length() will have to be > 0 at this point otherwise it would - // return at line 244. - auto d = ptTo0.Cross(line); - auto lineLength = line.Length(); - assert(lineLength > 0); - return d.Length() / lineLength; - } - - /// \brief Check if this line intersects the given line segment. - /// \param[in] _line The line to check for intersection. - /// \param[in] _epsilon The error bounds within which the intersection - /// check will return true. - /// \return True if an intersection was found. - public: bool Intersect(const Line3 &_line, - double _epsilon = 1e-6) const - { - static math::Vector3 ignore; - return this->Intersect(_line, ignore, _epsilon); - } - - /// \brief Test if this line and the given line are coplanar. - /// \param[in] _line Line to check against. - /// \param[in] _epsilon The error bounds within which the - /// check will return true. - /// \return True if the two lines are coplanar. - public: bool Coplanar(const Line3 &_line, - const double _epsilon = 1e-6) const - { - return std::abs((_line[0] - this->pts[0]).Dot( - (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0]))) - <= _epsilon; - } - - /// \brief Test if this line and the given line are parallel. - /// \param[in] _line Line to check against. - /// \param[in] _epsilon The error bounds within which the - /// check will return true. - /// \return True if the two lines are parallel. - public: bool Parallel(const Line3 &_line, - const double _epsilon = 1e-6) const - { - return (this->pts[1] - this->pts[0]).Cross( - _line[1] - _line[0]).Length() <= _epsilon; - } - - /// \brief Check if this line intersects the given line segment. The - /// point of intersection is returned in the _pt parameter. - /// \param[in] _line The line to check for intersection. - /// \param[out] _pt The point of intersection. This value is only - /// valid if the return value is true. - /// \param[in] _epsilon The error bounds within which the intersection - /// check will return true. - /// \return True if an intersection was found. - public: bool Intersect(const Line3 &_line, math::Vector3 &_pt, - double _epsilon = 1e-6) const - { - // Handle special case when lines are parallel - if (this->Parallel(_line, _epsilon)) - { - // Check if _line's starting point is on the line. - if (this->Within(_line[0], _epsilon)) - { - _pt = _line[0]; - return true; - } - // Check if _line's ending point is on the line. - else if (this->Within(_line[1], _epsilon)) - { - _pt = _line[1]; - return true; - } - // Otherwise return false. - else - return false; - } - - // Get the line that is the shortest distance between this and _line - math::Line3 distLine; - this->Distance(_line, distLine, _epsilon); - - // If the length of the line is less than epsilon, then they - // intersect. - if (distLine.Length() < _epsilon) - { - _pt = distLine[0]; - return true; - } - - return false; - } - - /// \brief Check if the given point is between the start and end - /// points of the line segment. - /// \param[in] _pt Point to check. - /// \param[in] _epsilon The error bounds within which the within - /// check will return true. - /// \return True if the point is on the segement. - public: bool Within(const math::Vector3 &_pt, - double _epsilon = 1e-6) const - { - return _pt.X() <= std::max(this->pts[0].X(), - this->pts[1].X()) + _epsilon && - _pt.X() >= std::min(this->pts[0].X(), - this->pts[1].X()) - _epsilon && - _pt.Y() <= std::max(this->pts[0].Y(), - this->pts[1].Y()) + _epsilon && - _pt.Y() >= std::min(this->pts[0].Y(), - this->pts[1].Y()) - _epsilon && - _pt.Z() <= std::max(this->pts[0].Z(), - this->pts[1].Z()) + _epsilon && - _pt.Z() >= std::min(this->pts[0].Z(), - this->pts[1].Z()) - _epsilon; - } - - /// \brief Equality operator. - /// \param[in] _line Line to compare for equality. - /// \return True if the given line is equal to this line - public: bool operator==(const Line3 &_line) const - { - return this->pts[0] == _line[0] && this->pts[1] == _line[1]; - } - - /// \brief Inequality operator. - /// \param[in] _line Line to compare for inequality. - /// \return True if the given line is not to this line - public: bool operator!=(const Line3 &_line) const - { - return !(*this == _line); - } - - /// \brief Get the start or end point. - /// \param[in] _index 0 = start point, 1 = end point. The _index - /// parameter is clamped to the range [0, 1]. - public: math::Vector3 operator[](const size_t _index) const - { - return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; - } - - /// \brief Stream extraction operator - /// \param[in] _out output stream - /// \param[in] _line Line3 to output - /// \return The stream - public: friend std::ostream &operator<<( - std::ostream &_out, const Line3 &_line) - { - _out << _line[0] << " " << _line[1]; - return _out; - } - - /// \brief Assignment operator - /// \param[in] _line a new value - /// \return this - public: Line3 &operator=(const Line3 &_line) - { - this->pts[0] = _line[0]; - this->pts[1] = _line[1]; - - return *this; - } - - /// \brief Vector for storing the start and end points of the line - private: math::Vector3 pts[2]; - }; - - typedef Line3 Line3i; - typedef Line3 Line3d; - typedef Line3 Line3f; - } - } -} -#endif diff --git a/include/ignition/math/MassMatrix3.hh b/include/ignition/math/MassMatrix3.hh index 4767ffab6..b79e8c16c 100644 --- a/include/ignition/math/MassMatrix3.hh +++ b/include/ignition/math/MassMatrix3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Open Source Robotics Foundation + * 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. @@ -13,1268 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MASSMATRIX3_HH_ -#define IGNITION_MATH_MASSMATRIX3_HH_ - -#include -#include -#include -#include + */ +#include #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Material.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Vector2.hh" -#include "ignition/math/Vector3.hh" -#include "ignition/math/Matrix3.hh" - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class MassMatrix3 MassMatrix3.hh ignition/math/MassMatrix3.hh - /// \brief A class for inertial information about a rigid body - /// consisting of the scalar mass and a 3x3 symmetric moment - /// of inertia matrix stored as two Vector3's. - template - class MassMatrix3 - { - /// \brief Default Constructor, which inializes the mass and moments - /// to zero. - public: MassMatrix3() : mass(0) - {} - - /// \brief Constructor. - /// \param[in] _mass Mass value in kg if using metric. - /// \param[in] _ixxyyzz Diagonal moments of inertia. - /// \param[in] _ixyxzyz Off-diagonal moments of inertia - public: MassMatrix3(const T &_mass, - const Vector3 &_ixxyyzz, - const Vector3 &_ixyxzyz) - : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz) - {} - - /// \brief Copy constructor. - /// \param[in] _m MassMatrix3 element to copy - public: MassMatrix3(const MassMatrix3 &_m) - : mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()), - Ixyxzyz(_m.OffDiagonalMoments()) - {} - - /// \brief Destructor. - public: virtual ~MassMatrix3() {} - - /// \brief Set the mass. - /// \param[in] _m New mass value. - /// \return True if the MassMatrix3 is valid. - /// \deprecated bool SetMass(const T &_m) - public: bool IGN_DEPRECATED(5.0) Mass(const T &_m) - { - return this->SetMass(_m); - } - - /// \brief Set the mass. - /// \param[in] _m New mass value. - /// \return True if the MassMatrix3 is valid. - public: bool SetMass(const T &_m) - { - this->mass = _m; - return this->IsValid(); - } - - /// \brief Get the mass - /// \return The mass value - public: T Mass() const - { - return this->mass; - } - - /// \brief Set the moment of inertia matrix. - /// \param[in] _ixx X second moment of inertia (MOI) about x axis. - /// \param[in] _iyy Y second moment of inertia about y axis. - /// \param[in] _izz Z second moment of inertia about z axis. - /// \param[in] _ixy XY inertia. - /// \param[in] _ixz XZ inertia. - /// \param[in] _iyz YZ inertia. - /// \return True if the MassMatrix3 is valid. - /// \deprecated see bool SetInertiaMatrix(const T &, const T &, const T &, - /// const T &, const T &, const T &) - public: bool IGN_DEPRECATED(5.0) InertiaMatrix( - const T &_ixx, const T &_iyy, const T &_izz, - const T &_ixy, const T &_ixz, const T &_iyz) - { - return this->SetInertiaMatrix(_ixx, _iyy, _izz, _ixy, _ixz, _iyz); - } - - /// \brief Set the moment of inertia matrix. - /// \param[in] _ixx X second moment of inertia (MOI) about x axis. - /// \param[in] _iyy Y second moment of inertia about y axis. - /// \param[in] _izz Z second moment of inertia about z axis. - /// \param[in] _ixy XY inertia. - /// \param[in] _ixz XZ inertia. - /// \param[in] _iyz YZ inertia. - /// \return True if the MassMatrix3 is valid. - public: bool SetInertiaMatrix( - const T &_ixx, const T &_iyy, const T &_izz, - const T &_ixy, const T &_ixz, const T &_iyz) - { - this->Ixxyyzz.Set(_ixx, _iyy, _izz); - this->Ixyxzyz.Set(_ixy, _ixz, _iyz); - return this->IsValid(); - } - - /// \brief Get the diagonal moments of inertia (Ixx, Iyy, Izz). - /// \return The diagonal moments. - public: Vector3 DiagonalMoments() const - { - return this->Ixxyyzz; - } - - /// \brief Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz). - /// \return The off-diagonal moments of inertia. - public: Vector3 OffDiagonalMoments() const - { - return this->Ixyxzyz; - } - - /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz). - /// \param[in] _ixxyyzz diagonal moments of inertia - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetDiagonalMoments(const Vector3 &_ixxyyzz) - public: bool IGN_DEPRECATED(5.0) DiagonalMoments( - const Vector3 &_ixxyyzz) - { - return this->SetDiagonalMoments(_ixxyyzz); - } - - /// \brief Set the diagonal moments of inertia (Ixx, Iyy, Izz). - /// \param[in] _ixxyyzz diagonal moments of inertia - /// \return True if the MassMatrix3 is valid. - public: bool SetDiagonalMoments(const Vector3 &_ixxyyzz) - { - this->Ixxyyzz = _ixxyyzz; - return this->IsValid(); - } - - /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz). - /// \param[in] _ixyxzyz off-diagonal moments of inertia - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) - public: bool IGN_DEPRECATED(5.0) OffDiagonalMoments( - const Vector3 &_ixyxzyz) - { - return this->SetOffDiagonalMoments(_ixyxzyz); - } - - /// \brief Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz). - /// \param[in] _ixyxzyz off-diagonal moments of inertia - /// \return True if the MassMatrix3 is valid. - public: bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) - { - this->Ixyxzyz = _ixyxzyz; - return this->IsValid(); - } - - /// \brief Get IXX - /// \return IXX value - /// \deprecated See T Ixx() const - public: T IGN_DEPRECATED(5.0) IXX() const - { - return this->Ixx(); - } - - /// \brief Get IXX - /// \return IXX value - public: T Ixx() const - { - return this->Ixxyyzz[0]; - } - - /// \brief Get IYY - /// \return IYY value - /// \deprecated See T Iyy() const - public: T IGN_DEPRECATED(5.0) IYY() const - { - return this->Iyy(); - } - - /// \brief Get IYY - /// \return IYY value - public: T Iyy() const - { - return this->Ixxyyzz[1]; - } - - /// \brief Get IZZ - /// \return IZZ value - /// \deprecated See T Izz() const - public: T IGN_DEPRECATED(5.0) IZZ() const - { - return this->Izz(); - } - - /// \brief Get IZZ - /// \return IZZ value - public: T Izz() const - { - return this->Ixxyyzz[2]; - } - - /// \brief Get IXY - /// \return IXY value - /// \deprecated See T Ixy() const - public: T IGN_DEPRECATED(5.0) IXY() const - { - return this->Ixy(); - } - - /// \brief Get IXY - /// \return IXY value - public: T Ixy() const - { - return this->Ixyxzyz[0]; - } - - /// \brief Get IXZ - /// \return IXZ value - /// \deprecated See T Ixz() const - public: T IGN_DEPRECATED(5.0) IXZ() const - { - return this->Ixz(); - } - - /// \brief Get IXZ - /// \return IXZ value - public: T Ixz() const - { - return this->Ixyxzyz[1]; - } - - /// \brief Get IYZ - /// \return IYZ value - /// \deprecated See T Iyz() const - public: T IGN_DEPRECATED(5.0) IYZ() const - { - return this->Iyz(); - } - - /// \brief Get IYZ - /// \return IYZ value - public: T Iyz() const - { - return this->Ixyxzyz[2]; - } - - /// \brief Set IXX - /// \param[in] _v IXX value - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetIxx(const T &_v) - public: bool IGN_DEPRECATED(5.0) IXX(const T &_v) - { - return this->SetIxx(_v); - } - - /// \brief Set IXX - /// \param[in] _v IXX value - /// \return True if the MassMatrix3 is valid. - public: bool SetIxx(const T &_v) - { - this->Ixxyyzz.X(_v); - return this->IsValid(); - } - - /// \brief Set IYY - /// \param[in] _v IYY value - /// \return True if the MassMatrix3 is valid. - /// \deprecated see bool SetIyy(const T &_v) - public: bool IGN_DEPRECATED(5.0) IYY(const T &_v) - { - return this->SetIyy(_v); - } - - /// \brief Set IYY - /// \param[in] _v IYY value - /// \return True if the MassMatrix3 is valid. - public: bool SetIyy(const T &_v) - { - this->Ixxyyzz.Y(_v); - return this->IsValid(); - } - - /// \brief Set IZZ - /// \param[in] _v IZZ value - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetIzz(const T &_v) - public: bool IGN_DEPRECATED(5.0) IZZ(const T &_v) - { - return this->SetIzz(_v); - } - - /// \brief Set IZZ - /// \param[in] _v IZZ value - /// \return True if the MassMatrix3 is valid. - public: bool SetIzz(const T &_v) - { - this->Ixxyyzz.Z(_v); - return this->IsValid(); - } - - /// \brief Set IXY - /// \param[in] _v IXY value - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetIxy(const T &_v) - public: bool IGN_DEPRECATED(5.0) IXY(const T &_v) - { - return this->SetIxy(_v); - } - - /// \brief Set IXY - /// \param[in] _v IXY value - /// \return True if the MassMatrix3 is valid. - public: bool SetIxy(const T &_v) - { - this->Ixyxzyz.X(_v); - return this->IsValid(); - } - - /// \brief Set IXZ - /// \param[in] _v IXZ value - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetIxz(const T &_v) - public: bool IGN_DEPRECATED(5.0) IXZ(const T &_v) - { - return this->SetIxz(_v); - } - - /// \brief Set IXZ - /// \param[in] _v IXZ value - /// \return True if the MassMatrix3 is valid. - public: bool SetIxz(const T &_v) - { - this->Ixyxzyz.Y(_v); - return this->IsValid(); - } - - /// \brief Set IYZ - /// \param[in] _v IYZ value - /// \return True if the MassMatrix3 is valid. - /// \deprecated See bool SetIyz(const T &_v) - public: bool IGN_DEPRECATED(5.0) IYZ(const T &_v) - { - return this->SetIyz(_v); - } - - /// \brief Set IYZ - /// \param[in] _v IYZ value - /// \return True if the MassMatrix3 is valid. - public: bool SetIyz(const T &_v) - { - this->Ixyxzyz.Z(_v); - return this->IsValid(); - } - - /// \brief returns Moments of Inertia as a Matrix3 - /// \return Moments of Inertia as a Matrix3 - /// \deprecated See Matrix3 Moi() const - public: Matrix3 IGN_DEPRECATED(5.0) MOI() const - { - return this->Moi(); - } - - /// \brief returns Moments of Inertia as a Matrix3 - /// \return Moments of Inertia as a Matrix3 - public: Matrix3 Moi() const - { - return Matrix3( - this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1], - this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2], - this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]); - } - - /// \brief Sets Moments of Inertia (MOI) from a Matrix3. - /// Symmetric component of input matrix is used by averaging - /// off-axis terms. - /// \param[in] _moi Moments of Inertia as a Matrix3 - /// \return True if the MassMatrix3 is valid. - /// \deprecated See SetMoi(const Matrix3 &_moi) - public: bool IGN_DEPRECATED(5.0) MOI(const Matrix3 &_moi) - { - return this->SetMoi(_moi); - } - - /// \brief Sets Moments of Inertia (MOI) from a Matrix3. - /// Symmetric component of input matrix is used by averaging - /// off-axis terms. - /// \param[in] _moi Moments of Inertia as a Matrix3 - /// \return True if the MassMatrix3 is valid. - public: bool SetMoi(const Matrix3 &_moi) - { - this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2)); - this->Ixyxzyz.Set( - 0.5*(_moi(0, 1) + _moi(1, 0)), - 0.5*(_moi(0, 2) + _moi(2, 0)), - 0.5*(_moi(1, 2) + _moi(2, 1))); - return this->IsValid(); - } - - /// \brief Equal operator. - /// \param[in] _massMatrix MassMatrix3 to copy. - /// \return Reference to this object. - public: MassMatrix3 &operator=(const MassMatrix3 &_massMatrix) - { - this->mass = _massMatrix.Mass(); - this->Ixxyyzz = _massMatrix.DiagonalMoments(); - this->Ixyxzyz = _massMatrix.OffDiagonalMoments(); - - return *this; - } - - /// \brief Equality comparison operator. - /// \param[in] _m MassMatrix3 to copy. - /// \return true if each component is equal within a default tolerance, - /// false otherwise - public: bool operator==(const MassMatrix3 &_m) const - { - return equal(this->mass, _m.Mass()) && - (this->Ixxyyzz == _m.DiagonalMoments()) && - (this->Ixyxzyz == _m.OffDiagonalMoments()); - } - - /// \brief Inequality test operator - /// \param[in] _m MassMatrix3 to test - /// \return True if not equal (using the default tolerance of 1e-6) - public: bool operator!=(const MassMatrix3 &_m) const - { - return !(*this == _m); - } - - /// \brief Verify that inertia values are positive semidefinite - /// - /// \param[in] _tolerance The amount of relative error to accept when - /// checking whether this MassMatrix3 has a valid mass and moment - /// of inertia. Refer to Epsilon() for a description of _tolerance. - /// - /// \return True if mass is nonnegative and moment of inertia matrix - /// is positive semidefinite. The following is how the return value is - /// calculated - /// - /// \code - /// const T epsilon = this->Epsilon(_tolerance); - /// return (this->mass + epsilon >= 0) && - /// (this->IXX() + epsilon >= 0) && - /// (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) + - /// epsilon >= 0) && - /// (this->Moi().Determinant() + epsilon >= 0); - /// \endcode - /// - public: bool IsNearPositive(const T _tolerance = - IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const - { - const T epsilon = this->Epsilon(_tolerance); - - // Check if mass and determinants of all upper left submatrices - // of moment of inertia matrix are positive - return (this->mass >= 0) && - (this->Ixx() + epsilon >= 0) && - (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) + - epsilon >= 0) && - (this->Moi().Determinant() + epsilon >= 0); - } - - /// \brief Verify that inertia values are positive definite - /// - /// \param[in] _tolerance The amount of error to accept when - /// checking whether this MassMatrix3 has a valid mass and moment - /// of inertia. Refer to Epsilon() for a description of _tolerance. - /// - /// \return True if mass is positive and moment of inertia matrix - /// is positive definite. The following is how the return value is - /// calculated - /// - /// \code - /// const T epsilon = this->Epsilon(_tolerance); - /// return (this->mass + epsilon > 0) && - /// (this->IXX() + epsilon > 0) && - /// (this->IXX() * this->IYY() - std::pow(this->IXY(), 2) + - /// epsilon > 0) && - /// (this->Moi().Determinant() + epsilon > 0); - /// \endcode - /// - public: bool IsPositive(const T _tolerance = - IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const - { - const T epsilon = this->Epsilon(_tolerance); - - // Check if mass and determinants of all upper left submatrices - // of moment of inertia matrix are positive - return (this->mass > 0) && - (this->Ixx() + epsilon > 0) && - (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) + - epsilon > 0) && - (this->Moi().Determinant() + epsilon > 0); - } - - /// - /// \brief \copybrief Epsilon(const Vector3&,const T) - /// - /// \param[in] _tolerance A factor that is used to adjust the return - /// value. A value of zero will cause the return value to be zero. - /// A good value is 10, which is also the - /// MASSMATRIX3_DEFAULT_TOLERANCE. - public: T Epsilon(const T _tolerance = - IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const - { - return Epsilon(this->DiagonalMoments(), _tolerance); - } - - /// \brief Get an epsilon value that represents the amount of - /// acceptable error in a MassMatrix3. The epsilon value - /// is related to machine precision multiplied by the largest possible - /// moment of inertia. - /// - /// This function is used by IsValid(), IsNearPositive(), IsPositive(), - /// and ValidMoments(). - /// - /// \param[in] _moments Principal moments of inertia. - /// \param[in] _tolerance A factor that is used to adjust the return - /// value. A value of zero will cause the return value to be zero. - /// A good value is 10, which is also the - /// MASSMATRIX3_DEFAULT_TOLERANCE. - /// - /// \return The epsilon value computed using: - /// - /// \code - /// T maxPossibleMoI = 0.5 * std::abs(_moments.Sum()); - /// return _tolerance * - /// std::numeric_limits::epsilon() * maxPossibleMoI; - /// \endcode - public: static T Epsilon(const Vector3 &_moments, - const T _tolerance = - IGN_MASSMATRIX3_DEFAULT_TOLERANCE) - { - // The following was borrowed heavily from: - // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h - - // Compute the maximum possible moment of inertia, which will be - // used to compute whether the moments are valid. - // - // The maximum moment of inertia is bounded by: - // trace / 3 <= maxPossibleMoi <= trace / 2. - // - // The trace of a matrix is the sum of the coefficients on the - // main diagonal. For a mass matrix, this is equal to - // ixx + iyy + izz, or _moments.Sum() for this function's - // implementation. - // - // It is okay if maxPossibleMoi == zero. - T maxPossibleMoI = 0.5 * std::abs(_moments.Sum()); - - // In order to check validity of the moments we need to use an - // epsilon value that is related to machine precision - // multiplied by the largest possible moment of inertia. - return _tolerance * - std::numeric_limits::epsilon() * maxPossibleMoI; - } - - /// \brief Verify that inertia values are positive semi-definite - /// and satisfy the triangle inequality. - /// - /// \param[in] _tolerance The amount of error to accept when - /// checking whether the MassMatrix3 has a valid mass and moment - /// of inertia. This value is passed on to IsNearPositive() and - /// ValidMoments(), which in turn pass the tolerance value to - /// Epsilon(). Refer to Epsilon() for a description of _tolerance. - /// - /// \return True if IsNearPositive(_tolerance) and - /// ValidMoments(this->PrincipalMoments(), _tolerance) both return true. - public: bool IsValid(const T _tolerance = - IGN_MASSMATRIX3_DEFAULT_TOLERANCE) const - { - return this->IsNearPositive(_tolerance) && - ValidMoments(this->PrincipalMoments(), _tolerance); - } - - /// \brief Verify that principal moments are positive - /// and satisfy the triangle inequality. - /// \param[in] _moments Principal moments of inertia. - /// \param[in] _tolerance The amount of error to accept when - /// checking whether the moments are positive and satisfy the triangle - /// inequality. Refer to Epsilon() for a description of _tolerance. - /// \return True if moments of inertia are positive - /// and satisfy the triangle inequality. The following is how the - /// return value is calculated. - /// - /// \code - /// T epsilon = this->Epsilon(_tolerance); - /// - /// return _moments[0] + epsilon >= 0 && - /// _moments[1] + epsilon >= 0 && - /// _moments[2] + epsilon >= 0 && - /// _moments[0] + _moments[1] + epsilon >= _moments[2] && - /// _moments[1] + _moments[2] + epsilon >= _moments[0] && - /// _moments[2] + _moments[0] + epsilon >= _moments[1]; - /// \endcode - public: static bool ValidMoments(const Vector3 &_moments, - const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE) - { - T epsilon = Epsilon(_moments, _tolerance); - - return _moments[0] + epsilon >= 0 && - _moments[1] + epsilon >= 0 && - _moments[2] + epsilon >= 0 && - _moments[0] + _moments[1] + epsilon >= _moments[2] && - _moments[1] + _moments[2] + epsilon >= _moments[0] && - _moments[2] + _moments[0] + epsilon >= _moments[1]; - } - - /// \brief Compute principal moments of inertia, - /// which are the eigenvalues of the moment of inertia matrix. - /// \param[in] _tol Relative tolerance given by absolute value - /// of _tol. - /// Negative values of _tol are interpreted as a flag that - /// causes principal moments to always be sorted from smallest - /// to largest. - /// \return Principal moments of inertia. - /// If the matrix is already diagonal and _tol is positive, - /// they are returned in the existing order. - /// Otherwise, the moments are sorted from smallest to largest. - public: Vector3 PrincipalMoments(const T _tol = 1e-6) const - { - // Compute tolerance relative to maximum value of inertia diagonal - T tol = _tol * this->Ixxyyzz.Max(); - if (this->Ixyxzyz.Equal(Vector3::Zero, tol)) - { - // Matrix is already diagonalized, return diagonal moments - return this->Ixxyyzz; - } - - // Algorithm based on http://arxiv.org/abs/1306.6291v4 - // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric - // Matrix, by Maarten Kronenburg - Vector3 Id(this->Ixxyyzz); - Vector3 Ip(this->Ixyxzyz); - // b = Ixx + Iyy + Izz - T b = Id.Sum(); - // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2 - T c = Id[0]*Id[1] - std::pow(Ip[0], 2) - + Id[0]*Id[2] - std::pow(Ip[1], 2) - + Id[1]*Id[2] - std::pow(Ip[2], 2); - // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz - T d = Id[0]*std::pow(Ip[2], 2) - + Id[1]*std::pow(Ip[1], 2) - + Id[2]*std::pow(Ip[0], 2) - - Id[0]*Id[1]*Id[2] - - 2*Ip[0]*Ip[1]*Ip[2]; - // p = b^2 - 3c - T p = std::pow(b, 2) - 3*c; - - // At this point, it is important to check that p is not close - // to zero, since its inverse is used to compute delta. - // In equation 4.7, p is expressed as a sum of squares - // that is only zero if the matrix is diagonal - // with identical principal moments. - // This check has no test coverage, since this function returns - // immediately if a diagonal matrix is detected. - if (p < std::pow(tol, 2)) - return b / 3.0 * Vector3::One; - - // q = 2b^3 - 9bc - 27d - T q = 2*std::pow(b, 3) - 9*b*c - 27*d; - - // delta = acos(q / (2 * p^(1.5))) - // additionally clamp the argument to [-1,1] - T delta = acos(clamp(0.5 * q / std::pow(p, 1.5), -1, 1)); - - // sort the moments from smallest to largest - T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0; - T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0; - T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0; - sort3(moment0, moment1, moment2); - return Vector3(moment0, moment1, moment2); - } - - /// \brief Compute rotational offset of principal axes. - /// \param[in] _tol Relative tolerance given by absolute value - /// of _tol. - /// Negative values of _tol are interpreted as a flag that - /// causes principal moments to always be sorted from smallest - /// to largest. - /// \return Quaternion representing rotational offset of principal axes. - /// With a rotation matrix constructed from this quaternion R(q) - /// and a diagonal matrix L with principal moments on the diagonal, - /// the original moment of inertia matrix MOI can be reconstructed - /// with MOI = R(q).Transpose() * L * R(q) - public: Quaternion PrincipalAxesOffset(const T _tol = 1e-6) const - { - Vector3 moments = this->PrincipalMoments(_tol); - // Compute tolerance relative to maximum value of inertia diagonal - T tol = _tol * this->Ixxyyzz.Max(); - if (moments.Equal(this->Ixxyyzz, tol) || - (math::equal(moments[0], moments[1], std::abs(tol)) && - math::equal(moments[0], moments[2], std::abs(tol)))) - { - // matrix is already aligned with principal axes - // or all three moments are approximately equal - // return identity rotation - return Quaternion::Identity; - } - - // Algorithm based on http://arxiv.org/abs/1306.6291v4 - // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric - // Matrix, by Maarten Kronenburg - // A real, symmetric matrix can be diagonalized by an orthogonal matrix - // (due to the finite-dimensional spectral theorem - // https://en.wikipedia.org/wiki/Spectral_theorem - // #Hermitian_maps_and_Hermitian_matrices ), - // and another name for orthogonal matrix is rotation matrix. - // Section 5 of the paper shows how to compute Euler angles - // phi1, phi2, and phi3 that map to a rotation matrix. - // In some cases, there are multiple possible values for a given angle, - // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc. - // Similar variable names are used to the paper so that the paper - // can be used as an additional reference. - - // f1, f2 defined in equations 5.5, 5.6 - Vector2 f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]); - Vector2 f2(this->Ixxyyzz[1] - this->Ixxyyzz[2], - -2*this->Ixyxzyz[2]); - - // Check if two moments are equal, since different equations are used - // The moments vector is already sorted, so just check adjacent values. - Vector2 momentsDiff(moments[0] - moments[1], - moments[1] - moments[2]); - - // index of unequal moment - int unequalMoment = -1; - if (equal(momentsDiff[0], 0, std::abs(tol))) - unequalMoment = 2; - else if (equal(momentsDiff[1], 0, std::abs(tol))) - unequalMoment = 0; - - if (unequalMoment >= 0) - { - // moments[1] is the repeated value - // it is not equal to moments[unequalMoment] - // momentsDiff3 = lambda - lambda3 - T momentsDiff3 = moments[1] - moments[unequalMoment]; - // eq 5.21: - // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3) - // s >= 0 since A_11 is in range [lambda, lambda3] - T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3; - // set phi3 to zero for repeated moments (eq 5.23) - T phi3 = 0; - // phi2 = +- acos(sqrt(s)) - // start with just the positive value - // also clamp the acos argument to prevent NaN's - T phi2 = acos(clamp(ClampedSqrt(s), -1, 1)); - - // The paper defines variables phi11 and phi12 - // which are candidate values of angle phi1. - // phi12 is straightforward to compute as a function of f2 and g2. - // eq 5.25: - Vector2 g2(momentsDiff3 * s, 0); - // combining eq 5.12 and 5.14, and subtracting psi2 - // instead of multiplying by its rotation matrix: - math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); - phi12.Normalize(); - - // The paragraph prior to equation 5.16 describes how to choose - // the candidate value of phi1 based on the length - // of the f1 and f2 vectors. - // * When |f1| != 0 and |f2| != 0, then one should choose the - // value of phi2 so that phi11 = phi12 - // * When |f1| == 0 and f2 != 0, then phi1 = phi12 - // phi11 can be ignored, and either sign of phi2 can be used - // * The case of |f2| == 0 can be ignored at this point in the code - // since having a repeated moment when |f2| == 0 implies that - // the matrix is diagonal. But this function returns a unit - // quaternion for diagonal matrices, so we can assume |f2| != 0 - // See MassMatrix3.ipynb for a more complete discussion. - // - // Since |f2| != 0, we only need to consider |f1| - // * |f1| == 0: phi1 = phi12 - // * |f1| != 0: choose phi2 so that phi11 == phi12 - // In either case, phi1 = phi12, - // and the sign of phi2 must be chosen to make phi11 == phi12 - T phi1 = phi12.Radian(); - - bool f1small = f1.SquaredLength() < std::pow(tol, 2); - if (!f1small) - { - // a: phi2 > 0 - // eq. 5.24 - Vector2 g1a(0, 0.5*momentsDiff3 * sin(2*phi2)); - // combining eq 5.11 and 5.13, and subtracting psi1 - // instead of multiplying by its rotation matrix: - math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol)); - phi11a.Normalize(); - - // b: phi2 < 0 - // eq. 5.24 - Vector2 g1b(0, 0.5*momentsDiff3 * sin(-2*phi2)); - // combining eq 5.11 and 5.13, and subtracting psi1 - // instead of multiplying by its rotation matrix: - math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol)); - phi11b.Normalize(); - - // choose sign of phi2 - // based on whether phi11a or phi11b is closer to phi12 - // use sin and cos to account for angle wrapping - T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2) - + std::pow(cos(phi1) - cos(phi11a.Radian()), 2); - T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2) - + std::pow(cos(phi1) - cos(phi11b.Radian()), 2); - if (errb < erra) - { - phi2 *= -1; - } - } - - // I determined these arguments using trial and error - Quaternion result = Quaternion(-phi1, -phi2, -phi3).Inverse(); - - // Previous equations assume repeated moments are at the beginning - // of the moments vector (moments[0] == moments[1]). - // We have the vectors sorted by size, so it's possible that the - // repeated moments are at the end (moments[1] == moments[2]). - // In this case (unequalMoment == 0), we apply an extra - // rotation that exchanges moment[0] and moment[2] - // Rotation matrix = [ 0 0 1] - // [ 0 1 0] - // [-1 0 0] - // That is equivalent to a 90 degree pitch - if (unequalMoment == 0) - result *= Quaternion(0, IGN_PI_2, 0); - - return result; - } - - // No repeated principal moments - // eq 5.1: - T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2) - +(this->Ixxyyzz[0] - moments[2]) - *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1])) - / ((moments[1] - moments[2]) * (moments[2] - moments[0])); - // value of w depends on v - T w; - if (v < std::abs(tol)) - { - // first sentence after eq 5.4: - // "In the case that v = 0, then w = 1." - w = 1; - } - else - { - // eq 5.2: - w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v) - / ((moments[0] - moments[1]) * v); - } - // initialize values of angle phi1, phi2, phi3 - T phi1 = 0; - // eq 5.3: start with positive value - T phi2 = acos(clamp(ClampedSqrt(v), -1, 1)); - // eq 5.4: start with positive value - T phi3 = acos(clamp(ClampedSqrt(w), -1, 1)); - - // compute g1, g2 for phi2,phi3 >= 0 - // equations 5.7, 5.8 - Vector2 g1( - 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3), - 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2)); - Vector2 g2( - (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v, - (moments[0]-moments[1])*sin(phi2)*sin(2*phi3)); - - // The paragraph prior to equation 5.16 describes how to choose - // the candidate value of phi1 based on the length - // of the f1 and f2 vectors. - // * The case of |f1| == |f2| == 0 implies a repeated moment, - // which should not be possible at this point in the code - // * When |f1| != 0 and |f2| != 0, then one should choose the - // value of phi2 so that phi11 = phi12 - // * When |f1| == 0 and f2 != 0, then phi1 = phi12 - // phi11 can be ignored, and either sign of phi2, phi3 can be used - // * When |f2| == 0 and f1 != 0, then phi1 = phi11 - // phi12 can be ignored, and either sign of phi2, phi3 can be used - bool f1small = f1.SquaredLength() < std::pow(tol, 2); - bool f2small = f2.SquaredLength() < std::pow(tol, 2); - if (f1small && f2small) - { - // this should never happen - // f1small && f2small implies a repeated moment - // return invalid quaternion - /// \todo Use a mock class to test this line - return Quaternion::Zero; - } - else if (f1small) - { - // use phi12 (equations 5.12, 5.14) - math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); - phi12.Normalize(); - phi1 = phi12.Radian(); - } - else if (f2small) - { - // use phi11 (equations 5.11, 5.13) - math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol)); - phi11.Normalize(); - phi1 = phi11.Radian(); - } - else - { - // check for when phi11 == phi12 - // eqs 5.11, 5.13: - math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol)); - phi11.Normalize(); - // eqs 5.12, 5.14: - math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol))); - phi12.Normalize(); - T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2) - + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2); - phi1 = phi11.Radian(); - math::Vector2 signsPhi23(1, 1); - // case a: phi2 <= 0 - { - Vector2 g1a = Vector2(1, -1) * g1; - Vector2 g2a = Vector2(1, -1) * g2; - math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol)); - math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol))); - phi11a.Normalize(); - phi12a.Normalize(); - T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2) - + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2); - if (erra < err) - { - err = erra; - phi1 = phi11a.Radian(); - signsPhi23.Set(-1, 1); - } - } - // case b: phi3 <= 0 - { - Vector2 g1b = Vector2(-1, 1) * g1; - Vector2 g2b = Vector2(1, -1) * g2; - math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol)); - math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol))); - phi11b.Normalize(); - phi12b.Normalize(); - T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2) - + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2); - if (errb < err) - { - err = errb; - phi1 = phi11b.Radian(); - signsPhi23.Set(1, -1); - } - } - // case c: phi2,phi3 <= 0 - { - Vector2 g1c = Vector2(-1, -1) * g1; - Vector2 g2c = g2; - math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol)); - math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol))); - phi11c.Normalize(); - phi12c.Normalize(); - T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2) - + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2); - if (errc < err) - { - phi1 = phi11c.Radian(); - signsPhi23.Set(-1, -1); - } - } - - // apply sign changes - phi2 *= signsPhi23[0]; - phi3 *= signsPhi23[1]; - } - - // I determined these arguments using trial and error - return Quaternion(-phi1, -phi2, -phi3).Inverse(); - } - - /// \brief Get dimensions and rotation offset of uniform box - /// with equivalent mass and moment of inertia. - /// To compute this, the Matrix3 is diagonalized. - /// The eigenvalues on the diagonal and the rotation offset - /// of the principal axes are returned. - /// \param[in] _size Dimensions of box aligned with principal axes. - /// \param[in] _rot Rotational offset of principal axes. - /// \param[in] _tol Relative tolerance. - /// \return True if box properties were computed successfully. - public: bool EquivalentBox(Vector3 &_size, - Quaternion &_rot, - const T _tol = 1e-6) const - { - if (!this->IsPositive(0)) - { - // inertia is not positive, cannot compute equivalent box - return false; - } - - Vector3 moments = this->PrincipalMoments(_tol); - if (!ValidMoments(moments)) - { - // principal moments don't satisfy the triangle identity - return false; - } - - // The reason for checking that the principal moments satisfy - // the triangle inequality - // I1 + I2 - I3 >= 0 - // is to ensure that the arguments to sqrt in these equations - // are positive and the box size is real. - _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass)); - _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass)); - _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass)); - - _rot = this->PrincipalAxesOffset(_tol); - - if (_rot == Quaternion::Zero) - { - // _rot is an invalid quaternion - /// \todo Use a mock class to test this line - return false; - } - - return true; - } - - /// \brief Set inertial properties based on a Material and equivalent box. - /// \param[in] _mat Material that specifies a density. Uniform density - /// is used. - /// \param[in] _size Size of equivalent box. - /// \param[in] _rot Rotational offset of equivalent box. - /// \return True if inertial properties were set successfully. - public: bool SetFromBox(const Material &_mat, - const Vector3 &_size, - const Quaternion &_rot = Quaternion::Identity) - { - T volume = _size.X() * _size.Y() * _size.Z(); - return this->SetFromBox(_mat.Density() * volume, _size, _rot); - } - - /// \brief Set inertial properties based on mass and equivalent box. - /// \param[in] _mass Mass to set. - /// \param[in] _size Size of equivalent box. - /// \param[in] _rot Rotational offset of equivalent box. - /// \return True if inertial properties were set successfully. - public: bool SetFromBox(const T _mass, - const Vector3 &_size, - const Quaternion &_rot = Quaternion::Identity) - { - // Check that _mass and _size are strictly positive - // and that quaternion is valid - if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion::Zero) - { - return false; - } - this->SetMass(_mass); - return this->SetFromBox(_size, _rot); - } - - /// \brief Set inertial properties based on equivalent box - /// using the current mass value. - /// \param[in] _size Size of equivalent box. - /// \param[in] _rot Rotational offset of equivalent box. - /// \return True if inertial properties were set successfully. - public: bool SetFromBox(const Vector3 &_size, - const Quaternion &_rot = Quaternion::Identity) - { - // Check that _mass and _size are strictly positive - // and that quaternion is valid - if (this->Mass() <= 0 || _size.Min() <= 0 || - _rot == Quaternion::Zero) - { - return false; - } - - // Diagonal matrix L with principal moments - Matrix3 L; - T x2 = std::pow(_size.X(), 2); - T y2 = std::pow(_size.Y(), 2); - T z2 = std::pow(_size.Z(), 2); - L(0, 0) = this->mass / 12.0 * (y2 + z2); - L(1, 1) = this->mass / 12.0 * (z2 + x2); - L(2, 2) = this->mass / 12.0 * (x2 + y2); - Matrix3 R(_rot); - return this->SetMoi(R * L * R.Transposed()); - } - - /// \brief Set inertial properties based on a Material and equivalent - /// cylinder aligned with Z axis. - /// \param[in] _mat Material that specifies a density. Uniform density - /// is used. - /// \param[in] _length Length of cylinder along Z axis. - /// \param[in] _radius Radius of cylinder. - /// \param[in] _rot Rotational offset of equivalent cylinder. - /// \return True if inertial properties were set successfully. - public: bool SetFromCylinderZ(const Material &_mat, - const T _length, - const T _radius, - const Quaternion &_rot = Quaternion::Identity) - { - // Check that density, _radius and _length are strictly positive - // and that quaternion is valid - if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 || - _rot == Quaternion::Zero) - { - return false; - } - T volume = IGN_PI * _radius * _radius * _length; - return this->SetFromCylinderZ(_mat.Density() * volume, - _length, _radius, _rot); - } - - /// \brief Set inertial properties based on mass and equivalent cylinder - /// aligned with Z axis. - /// \param[in] _mass Mass to set. - /// \param[in] _length Length of cylinder along Z axis. - /// \param[in] _radius Radius of cylinder. - /// \param[in] _rot Rotational offset of equivalent cylinder. - /// \return True if inertial properties were set successfully. - public: bool SetFromCylinderZ(const T _mass, - const T _length, - const T _radius, - const Quaternion &_rot = Quaternion::Identity) - { - // Check that _mass, _radius and _length are strictly positive - // and that quaternion is valid - if (_mass <= 0 || _length <= 0 || _radius <= 0 || - _rot == Quaternion::Zero) - { - return false; - } - this->SetMass(_mass); - return this->SetFromCylinderZ(_length, _radius, _rot); - } - - /// \brief Set inertial properties based on equivalent cylinder - /// aligned with Z axis using the current mass value. - /// \param[in] _length Length of cylinder along Z axis. - /// \param[in] _radius Radius of cylinder. - /// \param[in] _rot Rotational offset of equivalent cylinder. - /// \return True if inertial properties were set successfully. - public: bool SetFromCylinderZ(const T _length, - const T _radius, - const Quaternion &_rot) - { - // Check that _mass and _size are strictly positive - // and that quaternion is valid - if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 || - _rot == Quaternion::Zero) - { - return false; - } - - // Diagonal matrix L with principal moments - T radius2 = std::pow(_radius, 2); - Matrix3 L; - L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2)); - L(1, 1) = L(0, 0); - L(2, 2) = this->mass / 2.0 * radius2; - Matrix3 R(_rot); - return this->SetMoi(R * L * R.Transposed()); - } - - /// \brief Set inertial properties based on a material and - /// equivalent sphere. - /// \param[in] _mat Material that specifies a density. Uniform density - /// is used. - /// \param[in] _radius Radius of equivalent, uniform sphere. - /// \return True if inertial properties were set successfully. - public: bool SetFromSphere(const Material &_mat, const T _radius) - { - // Check that the density and _radius are strictly positive - if (_mat.Density() <= 0 || _radius <= 0) - { - return false; - } - - T volume = (4.0/3.0) * IGN_PI * std::pow(_radius, 3); - return this->SetFromSphere(_mat.Density() * volume, _radius); - } - - /// \brief Set inertial properties based on mass and equivalent sphere. - /// \param[in] _mass Mass to set. - /// \param[in] _radius Radius of equivalent, uniform sphere. - /// \return True if inertial properties were set successfully. - public: bool SetFromSphere(const T _mass, const T _radius) - { - // Check that _mass and _radius are strictly positive - if (_mass <= 0 || _radius <= 0) - { - return false; - } - this->SetMass(_mass); - return this->SetFromSphere(_radius); - } - - /// \brief Set inertial properties based on equivalent sphere - /// using the current mass value. - /// \param[in] _radius Radius of equivalent, uniform sphere. - /// \return True if inertial properties were set successfully. - public: bool SetFromSphere(const T _radius) - { - // Check that _mass and _radius are strictly positive - if (this->Mass() <= 0 || _radius <= 0) - { - return false; - } - - // Diagonal matrix L with principal moments - T radius2 = std::pow(_radius, 2); - Matrix3 L; - L(0, 0) = 0.4 * this->mass * radius2; - L(1, 1) = 0.4 * this->mass * radius2; - L(2, 2) = 0.4 * this->mass * radius2; - return this->SetMoi(L); - } - - /// \brief Square root of positive numbers, otherwise zero. - /// \param[in] _x Number to be square rooted. - /// \return sqrt(_x) if _x > 0, otherwise 0 - private: static inline T ClampedSqrt(const T &_x) - { - if (_x <= 0) - return 0; - return sqrt(_x); - } - - /// \brief Angle formed by direction of a Vector2. - /// \param[in] _v Vector whose direction is to be computed. - /// \param[in] _eps Minimum length of vector required for computing angle. - /// \return Angle formed between vector and X axis, - /// or zero if vector has length less than 1e-6. - private: static T Angle2(const Vector2 &_v, const T _eps = 1e-6) - { - if (_v.SquaredLength() < std::pow(_eps, 2)) - return 0; - return atan2(_v[1], _v[0]); - } - - /// \brief Mass of the object. Default is 0.0. - private: T mass; - - /// \brief Principal moments of inertia. Default is (0.0 0.0 0.0) - /// These Moments of Inertia are specified in the local frame. - /// Where Ixxyyzz.x is Ixx, Ixxyyzz.y is Iyy and Ixxyyzz.z is Izz. - private: Vector3 Ixxyyzz; - - /// \brief Product moments of inertia. Default is (0.0 0.0 0.0) - /// These MOI off-diagonals are specified in the local frame. - /// Where Ixyxzyz.x is Ixy, Ixyxzyz.y is Ixz and Ixyxzyz.z is Iyz. - private: Vector3 Ixyxzyz; - }; - - typedef MassMatrix3 MassMatrix3d; - typedef MassMatrix3 MassMatrix3f; - } - } -} -#endif diff --git a/include/ignition/math/Material.hh b/include/ignition/math/Material.hh index 89bde08ea..1e57794db 100644 --- a/include/ignition/math/Material.hh +++ b/include/ignition/math/Material.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,154 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MATERIAL_HH_ -#define IGNITION_MATH_MATERIAL_HH_ + */ -#include -#include -#include -#include +#include #include -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // Forward declarations - class MaterialPrivate; - - /// \brief Contains information about a single material. - /// - /// Steel, wood, and iron are examples of materials. This class - /// allows you to create custom materials, or use built-in materials. - /// The list of built-in materials can be found in the ::MaterialType - /// enum. - /// - /// This class will replace the - /// [MaterialDensity class](https://github.com/ignitionrobotics/ign-common/blob/ign-common1/include/ignition/common/MaterialDensity.hh) - /// found in the Ignition Common library, which was at version 1 at the - /// time of this writing. - /// - /// **How to create a wood material:** - /// - /// ~~~ - /// Material mat(MaterialType::WOOD); - /// std::cout << "The density of " << mat.Name() << " is " - /// << mat.Density() << std::endl; - /// ~~~ - /// - /// **How to create a custom material:** - /// - /// ~~~ - /// Material mat; - /// mat.SetDensity(12.23); - /// mat.SetName("my_material"); - /// std::cout << "The density of " << mat.Name() is " - /// << mat.Density() << std::endl; - /// ~~~ - class IGNITION_MATH_VISIBLE Material - { - /// \brief Constructor. - public: Material(); - - /// \brief Construct a material based on a type. - /// \param[in] _type Built-in type to create. - public: explicit Material(const MaterialType _type); - - /// \brief Construct a material based on a type name. - /// \param[in] _typename Name of the built-in type to create. String - /// names are listed in the ::MaterialType documentation. - public: explicit Material(const std::string &_typename); - - /// \brief Construct a material based on a density value. - /// \param[in] _density Material density. - public: explicit Material(const double _density); - - /// \brief Copy constructor. - /// \param[in] _material Material to copy. - public: Material(const Material &_material); - - /// \brief Move constructor. - /// \param[in] _material Material to move. The _material object will - /// have default values after the move. - public: Material(Material &&_material); - - /// \brief Destructor. - public: ~Material(); - - /// \brief Get all the built-in materials. - /// \return A map of all the materials. The map's key is - /// material type and the map's value is the material object. - public: static const std::map &Predefined(); - - /// \brief Set this Material to the built-in Material with - /// the nearest density value within _epsilon. If a built-in material - /// could not be found, then this Material is not changed. - /// \param[in] _value Density value of entry to match. - /// \param[in] _epsilon Allowable range of difference between _value, - /// and a material's density. - public: void SetToNearestDensity( - const double _value, - const double _epsilon = std::numeric_limits::max()); - - /// \brief Copy operator. - /// \param[in] _material Material to copy. - /// \return Reference to this material. - public: Material &operator=(const Material &_material); - - /// \brief Move operator. - /// \param[in] _material Material to move. The _material object will - /// contain default values after this move. - /// \return Reference to this Material. - public: Material &operator=(Material &&_material); - - /// \brief Equality operator. This compares type and density values. - /// \param[in] _material Material to evaluate this object against. - /// \return True if this material is equal to the given _material. - public: bool operator==(const Material &_material) const; - - /// \brief Inequality operator. This compares type and density values. - /// \param[in] _material Material to evaluate this object against. - /// \return True if this material is not equal to the given _material. - public: bool operator!=(const Material &_material) const; - - /// \brief Get the material's type. - /// \return The material's type. - public: MaterialType Type() const; - - /// \brief Set the material's type. This will only set the type value. - /// Other properties, such as density, will not be changed. - /// \param[in] _type The material's type. - public: void SetType(const MaterialType _type); - - /// \brief Get the name of the material. This will match the enum type - /// names used in ::MaterialType, but in lowercase, if a built-in - /// material is used. - /// \return The material's name. - /// \sa void SetName(const std::string &_name) - public: std::string Name() const; - - /// \brief Set the name of the material. - /// \param[in] _name The material's name. - /// \sa std::string Name() const - public: void SetName(const std::string &_name); - - /// \brief Get the density value of the material in kg/m^3. - /// \return The density of this material in kg/m^3. - public: double Density() const; - - /// \brief Set the density value of the material in kg/m^3. - /// \param[in] _density The density of this material in kg/m^3. - public: void SetDensity(const double _density); - - /// \brief Private data pointer. - private: MaterialPrivate *dataPtr = nullptr; - }; - } - } -} -#endif diff --git a/include/ignition/math/MaterialType.hh b/include/ignition/math/MaterialType.hh index eae22f3a4..a5cdb889f 100644 --- a/include/ignition/math/MaterialType.hh +++ b/include/ignition/math/MaterialType.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,87 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MATERIALTYPES_HH_ -#define IGNITION_MATH_MATERIALTYPES_HH_ + */ -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \enum MaterialType - /// \brief This enum lists the supported material types. A value can be - /// used to create a Material instance. - /// Source: https://en.wikipedia.org/wiki/Density - /// \sa Material - // Developer Note: When modifying this enum, make sure to also modify - // the kMaterials map in src/MaterialTypes.hh. - enum class MaterialType - { - /// \brief Styrofoam, density = 75.0 kg/m^3 - /// String name = "styrofoam" - STYROFOAM = 0, - - /// \brief Pine, density = 373.0 kg/m^3 - /// String name = "pine" - PINE, - - /// \brief Wood, density = 700.0 kg/m^3 - /// String name = "wood" - WOOD, - - /// \brief Oak, density = 710.0 kg/m^3 - /// String name = "oak" - OAK, - - /// \brief Plastic, density = 1175.0 kg/m^3 - /// String name = "plastic" - PLASTIC, - - /// \brief Concrete, density = 2000.0 kg/m^3 - /// String name = "concrete" - CONCRETE, - - /// \brief Aluminum, density = 2700.0 kg/m^3 - /// String name = "aluminum" - ALUMINUM, - - /// \brief Steel alloy, density = 7600.0 kg/m^3 - /// String name = "steel_alloy" - STEEL_ALLOY, - - /// \brief Stainless steel, density = 7800.0 kg/m^3 - /// String name = "steel_stainless" - STEEL_STAINLESS, - - /// \brief Iron, density = 7870.0 kg/m^3 - /// String name = "iron" - IRON, - - /// \brief Brass, density = 8600.0 kg/m^3 - /// String name = "brass" - BRASS, - - /// \brief Copper, density = 8940.0 kg/m^3 - /// String name = "copper" - COPPER, - - /// \brief Tungsten, density = 19300.0 kg/m^3 - /// String name = "tungsten" - TUNGSTEN, - - /// \brief Represents an invalid or unknown material. - // This value should always be last in the enum; it is used in - // MaterialDensity_TEST. - UNKNOWN_MATERIAL - }; - } - } -} -#endif diff --git a/include/ignition/math/Matrix3.hh b/include/ignition/math/Matrix3.hh index f101bb6ef..3deada8ff 100644 --- a/include/ignition/math/Matrix3.hh +++ b/include/ignition/math/Matrix3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,542 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MATRIX3_HH_ -#define IGNITION_MATH_MATRIX3_HH_ + */ -#include -#include -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - template class Quaternion; - - /// \class Matrix3 Matrix3.hh ignition/math/Matrix3.hh - /// \brief A 3x3 matrix class - template - class Matrix3 - { - /// \brief Identity matrix - public: static const Matrix3 Identity; - - /// \brief Zero matrix - public: static const Matrix3 Zero; - - /// \brief Constructor - public: Matrix3() - { - std::memset(this->data, 0, sizeof(this->data[0][0])*9); - } - - /// \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 - /// \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] _v10 Row 1, Col 0 value - /// \param[in] _v11 Row 1, Col 1 value - /// \param[in] _v12 Row 1, Col 2 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 - public: Matrix3(T _v00, T _v01, T _v02, - T _v10, T _v11, T _v12, - T _v20, T _v21, T _v22) - { - this->data[0][0] = _v00; - this->data[0][1] = _v01; - this->data[0][2] = _v02; - this->data[1][0] = _v10; - this->data[1][1] = _v11; - this->data[1][2] = _v12; - this->data[2][0] = _v20; - this->data[2][1] = _v21; - this->data[2][2] = _v22; - } - - /// \brief Construct Matrix3 from a quaternion. - /// \param[in] _q Quaternion. - public: explicit Matrix3(const Quaternion &_q) - { - Quaternion qt = _q; - qt.Normalize(); - this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(), - 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(), - 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(), - 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(), - 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(), - 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(), - 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(), - 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(), - 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y()); - } - - /// \brief Desctructor - public: virtual ~Matrix3() {} - - /// \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 - /// \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] _v20 Row 2, Col 0 value - /// \param[in] _v21 Row 2, Col 1 value - /// \param[in] _v22 Row 2, Col 2 value - public: void Set(T _v00, T _v01, T _v02, - T _v10, T _v11, T _v12, - T _v20, T _v21, T _v22) - { - this->data[0][0] = _v00; - this->data[0][1] = _v01; - this->data[0][2] = _v02; - this->data[1][0] = _v10; - this->data[1][1] = _v11; - this->data[1][2] = _v12; - this->data[2][0] = _v20; - this->data[2][1] = _v21; - 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) - { - this->Col(0, _xAxis); - this->Col(1, _yAxis); - this->Col(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) - { - T c = cos(_angle); - T s = sin(_angle); - T C = 1-c; - - this->data[0][0] = _axis.X()*_axis.X()*C + c; - this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s; - this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s; - - this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s; - this->data[1][1] = _axis.Y()*_axis.Y()*C + c; - this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s; - - this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s; - this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s; - this->data[2][2] = _axis.Z()*_axis.Z()*C + c; - } - - /// \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 From2Axes(const Vector3 &_v1, const Vector3 &_v2) - { - const T _v1LengthSquared = _v1.SquaredLength(); - if (_v1LengthSquared <= 0.0) - { - // zero vector - we can't handle this - this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); - return; - } - - const T _v2LengthSquared = _v2.SquaredLength(); - if (_v2LengthSquared <= 0.0) - { - // zero vector - we can't handle this - this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); - return; - } - - const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared); - if (fabs(dot - 1.0) <= 1e-6) - { - // the vectors are parallel - this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1); - return; - } - else if (fabs(dot + 1.0) <= 1e-6) - { - // the vectors are opposite - this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1); - return; - } - - const Vector3 cross = _v1.Cross(_v2).Normalize(); - - this->Axis(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. - public: void Col(unsigned int _c, const Vector3 &_v) - { - unsigned int c = clamp(_c, 0u, 2u); - - this->data[0][c] = _v.X(); - this->data[1][c] = _v.Y(); - this->data[2][c] = _v.Z(); - } - - /// \brief Equal operator. this = _mat - /// \param _mat Incoming matrix - /// \return itself - 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 - public: Matrix3 operator-(const Matrix3 &_m) const - { - return Matrix3( - this->data[0][0] - _m(0, 0), - this->data[0][1] - _m(0, 1), - this->data[0][2] - _m(0, 2), - this->data[1][0] - _m(1, 0), - this->data[1][1] - _m(1, 1), - this->data[1][2] - _m(1, 2), - this->data[2][0] - _m(2, 0), - this->data[2][1] - _m(2, 1), - this->data[2][2] - _m(2, 2)); - } - - /// \brief returns the element wise sum of two matrices - public: Matrix3 operator+(const Matrix3 &_m) const - { - return Matrix3( - this->data[0][0]+_m(0, 0), - this->data[0][1]+_m(0, 1), - this->data[0][2]+_m(0, 2), - this->data[1][0]+_m(1, 0), - this->data[1][1]+_m(1, 1), - this->data[1][2]+_m(1, 2), - this->data[2][0]+_m(2, 0), - this->data[2][1]+_m(2, 1), - this->data[2][2]+_m(2, 2)); - } - - /// \brief returns the element wise scalar multiplication - public: Matrix3 operator*(const T &_s) const - { - return Matrix3( - _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2], - _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2], - _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]); - } - - /// \brief Matrix multiplication operator - /// \param[in] _m Matrix3 to multiply - /// \return product of this * _m - public: Matrix3 operator*(const Matrix3 &_m) const - { - return Matrix3( - // first row - this->data[0][0]*_m(0, 0)+ - this->data[0][1]*_m(1, 0)+ - this->data[0][2]*_m(2, 0), - - this->data[0][0]*_m(0, 1)+ - this->data[0][1]*_m(1, 1)+ - this->data[0][2]*_m(2, 1), - - this->data[0][0]*_m(0, 2)+ - this->data[0][1]*_m(1, 2)+ - this->data[0][2]*_m(2, 2), - - // second row - this->data[1][0]*_m(0, 0)+ - this->data[1][1]*_m(1, 0)+ - this->data[1][2]*_m(2, 0), - - this->data[1][0]*_m(0, 1)+ - this->data[1][1]*_m(1, 1)+ - this->data[1][2]*_m(2, 1), - - this->data[1][0]*_m(0, 2)+ - this->data[1][1]*_m(1, 2)+ - this->data[1][2]*_m(2, 2), - - // third row - this->data[2][0]*_m(0, 0)+ - this->data[2][1]*_m(1, 0)+ - this->data[2][2]*_m(2, 0), - - this->data[2][0]*_m(0, 1)+ - this->data[2][1]*_m(1, 1)+ - this->data[2][2]*_m(2, 1), - - this->data[2][0]*_m(0, 2)+ - this->data[2][1]*_m(1, 2)+ - this->data[2][2]*_m(2, 2)); - } - - /// \brief Multiplication operator with Vector3 on the right - /// treated like a column vector. - /// \param _vec Vector3 - /// \return Resulting vector from multiplication - public: Vector3 operator*(const Vector3 &_vec) const - { - return Vector3( - this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() + - this->data[0][2]*_vec.Z(), - this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() + - this->data[1][2]*_vec.Z(), - this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() + - this->data[2][2]*_vec.Z()); - } - - /// \brief Matrix multiplication operator for scaling. - /// \param[in] _s Scaling factor. - /// \param[in] _m Input matrix. - /// \return A scaled matrix. - public: friend inline Matrix3 operator*(T _s, const Matrix3 &_m) - { - return _m * _s; - } - - /// \brief Matrix left multiplication operator for Vector3. - /// Treats the Vector3 like a row vector multiplying the matrix - /// from the left. - /// \param[in] _v Input vector. - /// \param[in] _m Input matrix. - /// \return The product vector. - public: friend inline Vector3 operator*(const Vector3 &_v, - const Matrix3 &_m) - { - return Vector3( - _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(), - _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(), - _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z()); - } - - /// \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 Matrix3 &_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[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[2][0], _m(2, 0), _tol) - && equal(this->data[2][1], _m(2, 1), _tol) - && 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) - public: bool operator==(const Matrix3 &_m) const - { - return this->Equal(_m, static_cast(1e-6)); - } - - /// \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) - { - 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) - public: bool operator!=(const Matrix3 &_m) const - { - return !(*this == _m); - } - - /// \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 - { - 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 - /// \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 T &operator()(size_t _row, size_t _col) - { - return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] - [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; - } - - /// \brief Return the determinant of the matrix - /// \return Determinant of this matrix. - public: T Determinant() const - { - T t0 = this->data[2][2]*this->data[1][1] - - this->data[2][1]*this->data[1][2]; - - T t1 = -(this->data[2][2]*this->data[1][0] - -this->data[2][0]*this->data[1][2]); - - T t2 = this->data[2][1]*this->data[1][0] - - this->data[2][0]*this->data[1][1]; - - return t0 * this->data[0][0] - + t1 * this->data[0][1] - + t2 * this->data[0][2]; - } - - /// \brief Return the inverse matrix - /// \return Inverse of this matrix. - public: Matrix3 Inverse() const - { - T t0 = this->data[2][2]*this->data[1][1] - - this->data[2][1]*this->data[1][2]; - - T t1 = -(this->data[2][2]*this->data[1][0] - - this->data[2][0]*this->data[1][2]); - - T t2 = this->data[2][1]*this->data[1][0] - - this->data[2][0]*this->data[1][1]; - - T invDet = 1.0 / (t0 * this->data[0][0] + - t1 * this->data[0][1] + - t2 * this->data[0][2]); - - return invDet * Matrix3( - t0, - - (this->data[2][2] * this->data[0][1] - - this->data[2][1] * this->data[0][2]), - + (this->data[1][2] * this->data[0][1] - - this->data[1][1] * this->data[0][2]), - t1, - + (this->data[2][2] * this->data[0][0] - - this->data[2][0] * this->data[0][2]), - - (this->data[1][2] * this->data[0][0] - - this->data[1][0] * this->data[0][2]), - t2, - - (this->data[2][1] * this->data[0][0] - - this->data[2][0] * this->data[0][1]), - + (this->data[1][1] * this->data[0][0] - - this->data[1][0] * this->data[0][1])); - } - - /// \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[1][2], this->data[2][1]); - } - - /// \brief Return the transpose of this matrix - /// \return Transpose of this matrix. - public: Matrix3 Transposed() const - { - return Matrix3( - this->data[0][0], this->data[1][0], this->data[2][0], - this->data[0][1], this->data[1][1], this->data[2][1], - 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 - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Matrix3 &_m) - { - _out << precision(_m(0, 0), 6) << " " - << precision(_m(0, 1), 6) << " " - << precision(_m(0, 2), 6) << " " - << precision(_m(1, 0), 6) << " " - << precision(_m(1, 1), 6) << " " - << precision(_m(1, 2), 6) << " " - << precision(_m(2, 0), 6) << " " - << precision(_m(2, 1), 6) << " " - << precision(_m(2, 2), 6); - - return _out; - } - - /// \brief Stream extraction operator - /// \param[in,out] _in input stream - /// \param[out] _m Matrix3 to read values into - /// \return the stream - public: friend std::istream &operator>>( - std::istream &_in, ignition::math::Matrix3 &_m) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - T d[9]; - _in >> d[0] >> d[1] >> d[2] - >> d[3] >> d[4] >> d[5] - >> d[6] >> d[7] >> d[8]; - - if (!_in.fail()) - { - _m.Set(d[0], d[1], d[2], - d[3], d[4], d[5], - d[6], d[7], d[8]); - } - return _in; - } - - /// \brief the 3x3 matrix - private: T data[3][3]; - }; - - template - const Matrix3 Matrix3::Identity( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - - template - const Matrix3 Matrix3::Zero( - 0, 0, 0, - 0, 0, 0, - 0, 0, 0); - - typedef Matrix3 Matrix3i; - typedef Matrix3 Matrix3d; - typedef Matrix3 Matrix3f; - } - } -} - -#endif diff --git a/include/ignition/math/Matrix4.hh b/include/ignition/math/Matrix4.hh index b7b0044c2..4f5359cc0 100644 --- a/include/ignition/math/Matrix4.hh +++ b/include/ignition/math/Matrix4.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,918 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MATRIX4_HH_ -#define IGNITION_MATH_MATRIX4_HH_ + */ -#include -#include -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Matrix4 Matrix4.hh ignition/math/Matrix4.hh - /// \brief A 4x4 matrix class - template - class Matrix4 - { - /// \brief Identity matrix - public: static const Matrix4 Identity; - - /// \brief Zero matrix - public: static const Matrix4 Zero; - - /// \brief Constructor - public: Matrix4() - { - memset(this->data, 0, sizeof(this->data[0][0])*16); - } - - /// \brief Copy constructor - /// \param _m Matrix to copy - public: Matrix4(const Matrix4 &_m) - { - memcpy(this->data, _m.data, sizeof(this->data[0][0])*16); - } - - /// \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] _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: Matrix4(T _v00, T _v01, T _v02, T _v03, - T _v10, T _v11, T _v12, T _v13, - T _v20, T _v21, T _v22, T _v23, - T _v30, T _v31, T _v32, T _v33) - { - this->Set(_v00, _v01, _v02, _v03, - _v10, _v11, _v12, _v13, - _v20, _v21, _v22, _v23, - _v30, _v31, _v32, _v33); - } - - /// \brief Construct Matrix4 from a quaternion. - /// \param[in] _q Quaternion. - public: explicit Matrix4(const Quaternion &_q) - { - Quaternion qt = _q; - qt.Normalize(); - this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(), - 2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(), - 2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(), - 0, - - 2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(), - 1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(), - 2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(), - 0, - - 2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(), - 2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(), - 1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(), - 0, - - 0, 0, 0, 1); - } - - /// \brief Construct Matrix4 from a math::Pose3 - /// \param[in] _pose Pose. - public: explicit Matrix4(const Pose3 &_pose) : Matrix4(_pose.Rot()) - { - this->SetTranslation(_pose.Pos()); - } - - /// \brief Destructor - public: virtual ~Matrix4() {} - - /// \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 _v10, T _v11, T _v12, T _v13, - T _v20, T _v21, T _v22, T _v23, - T _v30, T _v31, T _v32, T _v33) - { - this->data[0][0] = _v00; - this->data[0][1] = _v01; - this->data[0][2] = _v02; - this->data[0][3] = _v03; - - this->data[1][0] = _v10; - this->data[1][1] = _v11; - this->data[1][2] = _v12; - this->data[1][3] = _v13; - - this->data[2][0] = _v20; - this->data[2][1] = _v21; - this->data[2][2] = _v22; - this->data[2][3] = _v23; - - this->data[3][0] = _v30; - this->data[3][1] = _v31; - this->data[3][2] = _v32; - this->data[3][3] = _v33; - } - - /// \brief Set the upper-left 3x3 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) - { - T c = cos(_angle); - T s = sin(_angle); - T C = 1-c; - - this->data[0][0] = _axis.X()*_axis.X()*C + c; - this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s; - this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s; - - this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s; - this->data[1][1] = _axis.Y()*_axis.Y()*C + c; - this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s; - - this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s; - this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s; - this->data[2][2] = _axis.Z()*_axis.Z()*C + c; - } - - /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] - /// \param[in] _t Values to set - /// \deprecated Use SetTranslation instead - public: void - IGN_DEPRECATED(4) - Translate(const Vector3 &_t) - { - this->SetTranslation(_t); - } - - /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] - /// \param[in] _t Values to set - public: void SetTranslation(const Vector3 &_t) - { - this->data[0][3] = _t.X(); - this->data[1][3] = _t.Y(); - this->data[2][3] = _t.Z(); - } - - /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] - /// \param[in] _x X translation value. - /// \param[in] _y Y translation value. - /// \param[in] _z Z translation value. - /// \deprecated Use SetTranslation instead - public: void - IGN_DEPRECATED(4) - Translate(T _x, T _y, T _z) - { - this->SetTranslation(_x, _y, _z); - } - - /// \brief Set the translational values [ (0, 3) (1, 3) (2, 3) ] - /// \param[in] _x X translation value. - /// \param[in] _y Y translation value. - /// \param[in] _z Z translation value. - public: void SetTranslation(T _x, T _y, T _z) - { - this->data[0][3] = _x; - this->data[1][3] = _y; - this->data[2][3] = _z; - } - - /// \brief Get the translational values as a Vector3 - /// \return x,y,z translation values - public: Vector3 Translation() const - { - return Vector3(this->data[0][3], this->data[1][3], this->data[2][3]); - } - - /// \brief Get the scale values as a Vector3 - /// \return x,y,z scale values - public: Vector3 Scale() const - { - return Vector3(this->data[0][0], this->data[1][1], this->data[2][2]); - } - - /// \brief Get the rotation as a quaternion - /// \return the rotation - public: Quaternion Rotation() const - { - Quaternion q; - /// algorithm from Ogre::Quaternion source, which in turn is based on - /// Ken Shoemake's article "Quaternion Calculus and Fast Animation". - T trace = this->data[0][0] + this->data[1][1] + this->data[2][2]; - T root; - if (trace > 0) - { - root = sqrt(trace + 1.0); - q.W(root / 2.0); - root = 1.0 / (2.0 * root); - q.X((this->data[2][1] - this->data[1][2]) * root); - q.Y((this->data[0][2] - this->data[2][0]) * root); - q.Z((this->data[1][0] - this->data[0][1]) * root); - } - else - { - static unsigned int s_iNext[3] = {1, 2, 0}; - unsigned int i = 0; - if (this->data[1][1] > this->data[0][0]) - i = 1; - if (this->data[2][2] > this->data[i][i]) - i = 2; - unsigned int j = s_iNext[i]; - unsigned int k = s_iNext[j]; - - root = sqrt(this->data[i][i] - this->data[j][j] - - this->data[k][k] + 1.0); - - T a, b, c; - a = root / 2.0; - root = 1.0 / (2.0 * root); - b = (this->data[j][i] + this->data[i][j]) * root; - c = (this->data[k][i] + this->data[i][k]) * root; - - switch (i) - { - default: - case 0: q.X(a); break; - case 1: q.Y(a); break; - case 2: q.Z(a); break; - }; - switch (j) - { - default: - case 0: q.X(b); break; - case 1: q.Y(b); break; - case 2: q.Z(b); break; - }; - switch (k) - { - default: - case 0: q.X(c); break; - case 1: q.Y(c); break; - case 2: q.Z(c); break; - }; - - q.W((this->data[k][j] - this->data[j][k]) * root); - } - - return q; - } - - /// \brief Get the rotation as a Euler angles - /// \param[in] _firstSolution True to get the first Euler solution, - /// false to get the second. - /// \return the rotation - public: Vector3 EulerRotation(bool _firstSolution) const - { - Vector3 euler; - Vector3 euler2; - - T m31 = this->data[2][0]; - T m11 = this->data[0][0]; - T m12 = this->data[0][1]; - T m13 = this->data[0][2]; - T m32 = this->data[2][1]; - T m33 = this->data[2][2]; - T m21 = this->data[1][0]; - - if (std::abs(m31) >= 1.0) - { - euler.Z(0.0); - euler2.Z(0.0); - - if (m31 < 0.0) - { - euler.Y(IGN_PI / 2.0); - euler2.Y(IGN_PI / 2.0); - euler.X(atan2(m12, m13)); - euler2.X(atan2(m12, m13)); - } - else - { - euler.Y(-IGN_PI / 2.0); - euler2.Y(-IGN_PI / 2.0); - euler.X(atan2(-m12, -m13)); - euler2.X(atan2(-m12, -m13)); - } - } - else - { - euler.Y(-asin(m31)); - euler2.Y(IGN_PI - euler.Y()); - - euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y()))); - euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y()))); - - euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y()))); - euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y()))); - } - - if (_firstSolution) - return euler; - else - return euler2; - } - - /// \brief Get the transformation as math::Pose - /// \return the pose - public: Pose3 Pose() const - { - return Pose3(this->Translation(), this->Rotation()); - } - - /// \brief Set the scale - /// \param[in] _s scale - public: void Scale(const Vector3 &_s) - { - this->data[0][0] = _s.X(); - this->data[1][1] = _s.Y(); - this->data[2][2] = _s.Z(); - this->data[3][3] = 1.0; - } - - /// \brief Set the scale - /// \param[in] _x X scale value. - /// \param[in] _y Y scale value. - /// \param[in] _z Z scale value. - public: void Scale(T _x, T _y, T _z) - { - this->data[0][0] = _x; - this->data[1][1] = _y; - this->data[2][2] = _z; - this->data[3][3] = 1.0; - } - - /// \brief Return true if the matrix is affine - /// \return true if the matrix is affine, false otherwise - public: bool IsAffine() const - { - return equal(this->data[3][0], static_cast(0)) && - equal(this->data[3][1], static_cast(0)) && - equal(this->data[3][2], static_cast(0)) && - equal(this->data[3][3], static_cast(1)); - } - - /// \brief Perform an affine transformation - /// \param[in] _v Vector3 value for the transformation - /// \return The result of the transformation. A default constructed - /// Vector3 is returned if this matrix is not affine. - /// \deprecated Use bool TransformAffine(const Vector3 &_v, - /// Vector3 &_result) const; - public: Vector3 - IGN_DEPRECATED(3.0) - TransformAffine(const Vector3 &_v) const - { - if (this->IsAffine()) - { - return Vector3(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + - this->data[0][2]*_v.Z() + this->data[0][3], - this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + - this->data[1][2]*_v.Z() + this->data[1][3], - this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + - this->data[2][2]*_v.Z() + this->data[2][3]); - } - else - { - return Vector3(); - } - } - - /// \brief Perform an affine transformation - /// \param[in] _v Vector3 value for the transformation - /// \param[out] _result The result of the transformation. _result is - /// not changed if this matrix is not affine. - /// \return True if this matrix is affine, false otherwise. - public: bool TransformAffine(const Vector3 &_v, - Vector3 &_result) const - { - if (!this->IsAffine()) - return false; - - _result.Set(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() + - this->data[0][2]*_v.Z() + this->data[0][3], - this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() + - this->data[1][2]*_v.Z() + this->data[1][3], - this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() + - this->data[2][2]*_v.Z() + this->data[2][3]); - return true; - } - - /// \brief Return the determinant of the matrix - /// \return Determinant of this matrix. - public: T Determinant() const - { - T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; - - v0 = this->data[2][0]*this->data[3][1] - - this->data[2][1]*this->data[3][0]; - v1 = this->data[2][0]*this->data[3][2] - - this->data[2][2]*this->data[3][0]; - v2 = this->data[2][0]*this->data[3][3] - - this->data[2][3]*this->data[3][0]; - v3 = this->data[2][1]*this->data[3][2] - - this->data[2][2]*this->data[3][1]; - v4 = this->data[2][1]*this->data[3][3] - - this->data[2][3]*this->data[3][1]; - v5 = this->data[2][2]*this->data[3][3] - - this->data[2][3]*this->data[3][2]; - - t00 = v5*this->data[1][1] - v4*this->data[1][2] + v3*this->data[1][3]; - t10 = -v5*this->data[1][0] + v2*this->data[1][2] - v1*this->data[1][3]; - t20 = v4*this->data[1][0] - v2*this->data[1][1] + v0*this->data[1][3]; - t30 = -v3*this->data[1][0] + v1*this->data[1][1] - v0*this->data[1][2]; - - return t00 * this->data[0][0] - + t10 * this->data[0][1] - + t20 * this->data[0][2] - + t30 * this->data[0][3]; - } - - /// \brief Return the inverse matrix. - /// This is a non-destructive operation. - /// \return Inverse of this matrix. - public: Matrix4 Inverse() const - { - T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30; - Matrix4 r; - - v0 = this->data[2][0]*this->data[3][1] - - this->data[2][1]*this->data[3][0]; - v1 = this->data[2][0]*this->data[3][2] - - this->data[2][2]*this->data[3][0]; - v2 = this->data[2][0]*this->data[3][3] - - this->data[2][3]*this->data[3][0]; - v3 = this->data[2][1]*this->data[3][2] - - this->data[2][2]*this->data[3][1]; - v4 = this->data[2][1]*this->data[3][3] - - this->data[2][3]*this->data[3][1]; - v5 = this->data[2][2]*this->data[3][3] - - this->data[2][3]*this->data[3][2]; - - t00 = +(v5*this->data[1][1] - - v4*this->data[1][2] + v3*this->data[1][3]); - t10 = -(v5*this->data[1][0] - - v2*this->data[1][2] + v1*this->data[1][3]); - t20 = +(v4*this->data[1][0] - - v2*this->data[1][1] + v0*this->data[1][3]); - t30 = -(v3*this->data[1][0] - - v1*this->data[1][1] + v0*this->data[1][2]); - - T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] + - t20 * this->data[0][2] + t30 * this->data[0][3]); - - r(0, 0) = t00 * invDet; - r(1, 0) = t10 * invDet; - r(2, 0) = t20 * invDet; - r(3, 0) = t30 * invDet; - - r(0, 1) = -(v5*this->data[0][1] - - v4*this->data[0][2] + v3*this->data[0][3]) * invDet; - r(1, 1) = +(v5*this->data[0][0] - - v2*this->data[0][2] + v1*this->data[0][3]) * invDet; - r(2, 1) = -(v4*this->data[0][0] - - v2*this->data[0][1] + v0*this->data[0][3]) * invDet; - r(3, 1) = +(v3*this->data[0][0] - - v1*this->data[0][1] + v0*this->data[0][2]) * invDet; - - v0 = this->data[1][0]*this->data[3][1] - - this->data[1][1]*this->data[3][0]; - v1 = this->data[1][0]*this->data[3][2] - - this->data[1][2]*this->data[3][0]; - v2 = this->data[1][0]*this->data[3][3] - - this->data[1][3]*this->data[3][0]; - v3 = this->data[1][1]*this->data[3][2] - - this->data[1][2]*this->data[3][1]; - v4 = this->data[1][1]*this->data[3][3] - - this->data[1][3]*this->data[3][1]; - v5 = this->data[1][2]*this->data[3][3] - - this->data[1][3]*this->data[3][2]; - - r(0, 2) = +(v5*this->data[0][1] - - v4*this->data[0][2] + v3*this->data[0][3]) * invDet; - r(1, 2) = -(v5*this->data[0][0] - - v2*this->data[0][2] + v1*this->data[0][3]) * invDet; - r(2, 2) = +(v4*this->data[0][0] - - v2*this->data[0][1] + v0*this->data[0][3]) * invDet; - r(3, 2) = -(v3*this->data[0][0] - - v1*this->data[0][1] + v0*this->data[0][2]) * invDet; - - v0 = this->data[2][1]*this->data[1][0] - - this->data[2][0]*this->data[1][1]; - v1 = this->data[2][2]*this->data[1][0] - - this->data[2][0]*this->data[1][2]; - v2 = this->data[2][3]*this->data[1][0] - - this->data[2][0]*this->data[1][3]; - v3 = this->data[2][2]*this->data[1][1] - - this->data[2][1]*this->data[1][2]; - v4 = this->data[2][3]*this->data[1][1] - - this->data[2][1]*this->data[1][3]; - v5 = this->data[2][3]*this->data[1][2] - - this->data[2][2]*this->data[1][3]; - - r(0, 3) = -(v5*this->data[0][1] - - v4*this->data[0][2] + v3*this->data[0][3]) * invDet; - r(1, 3) = +(v5*this->data[0][0] - - v2*this->data[0][2] + v1*this->data[0][3]) * invDet; - r(2, 3) = -(v4*this->data[0][0] - - v2*this->data[0][1] + v0*this->data[0][3]) * invDet; - r(3, 3) = +(v3*this->data[0][0] - - v1*this->data[0][1] + v0*this->data[0][2]) * invDet; - - return r; - } - - /// \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[1][2], this->data[2][1]); - std::swap(this->data[1][3], this->data[3][1]); - std::swap(this->data[2][3], this->data[3][2]); - } - - /// \brief Return the transpose of this matrix - /// \return Transpose of this matrix. - public: Matrix4 Transposed() const - { - return Matrix4( - this->data[0][0], this->data[1][0], this->data[2][0], this->data[3][0], - this->data[0][1], this->data[1][1], this->data[2][1], this->data[3][1], - this->data[0][2], this->data[1][2], this->data[2][2], this->data[3][2], - this->data[0][3], this->data[1][3], this->data[2][3], this->data[3][3]); - } - - /// \brief Equal operator. this = _mat - /// \param _mat Incoming matrix - /// \return itself - public: Matrix4 &operator=(const Matrix4 &_mat) - { - memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16); - return *this; - } - - /// \brief Equal operator for 3x3 matrix - /// \param _mat Incoming matrix - /// \return itself - public: const Matrix4 &operator=(const Matrix3 &_mat) - { - this->data[0][0] = _mat(0, 0); - this->data[0][1] = _mat(0, 1); - this->data[0][2] = _mat(0, 2); - - this->data[1][0] = _mat(1, 0); - this->data[1][1] = _mat(1, 1); - this->data[1][2] = _mat(1, 2); - - this->data[2][0] = _mat(2, 0); - this->data[2][1] = _mat(2, 1); - this->data[2][2] = _mat(2, 2); - - return *this; - } - - /// \brief Multiplication assignment operator. This matrix will - /// become equal to this * _m2. - /// \param[in] _m2 Incoming matrix. - /// \return This matrix * _mat. - public: Matrix4 operator*=(const Matrix4 &_m2) - { - (*this) = (*this) * _m2; - return *this; - } - - /// \brief Multiplication operator - /// \param[in] _m2 Incoming matrix - /// \return This matrix * _mat - public: Matrix4 operator*(const Matrix4 &_m2) const - { - return Matrix4( - 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][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][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][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[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][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][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][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[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][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][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][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[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][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][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][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)); - } - - /// \brief Multiplication operator - /// \param _vec Vector3 - /// \return Resulting vector from multiplication - public: Vector3 operator*(const Vector3 &_vec) const - { - return Vector3( - this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() + - this->data[0][2]*_vec.Z() + this->data[0][3], - this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() + - this->data[1][2]*_vec.Z() + this->data[1][3], - this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() + - this->data[2][2]*_vec.Z() + this->data[2][3]); - } - - /// \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_THREE_SIZE_T)][ - clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_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_THREE_SIZE_T)] - [clamp(_col, IGN_ZERO_SIZE_T, IGN_THREE_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 Matrix4 &_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[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[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[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); - } - - /// \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 Matrix4 &_m) const - { - return this->Equal(_m, static_cast(1e-6)); - } - - /// \brief Inequality test operator - /// \param[in] _m Matrix4 to test - /// \return True if not equal (using the default tolerance of 1e-6) - public: bool operator!=(const Matrix4 &_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::Matrix4 &_m) - { - _out << precision(_m(0, 0), 6) << " " - << precision(_m(0, 1), 6) << " " - << precision(_m(0, 2), 6) << " " - << precision(_m(0, 3), 6) << " " - << precision(_m(1, 0), 6) << " " - << precision(_m(1, 1), 6) << " " - << precision(_m(1, 2), 6) << " " - << precision(_m(1, 3), 6) << " " - << precision(_m(2, 0), 6) << " " - << precision(_m(2, 1), 6) << " " - << precision(_m(2, 2), 6) << " " - << precision(_m(2, 3), 6) << " " - << precision(_m(3, 0), 6) << " " - << precision(_m(3, 1), 6) << " " - << precision(_m(3, 2), 6) << " " - << precision(_m(3, 3), 6); - - return _out; - } - - /// \brief Stream extraction operator - /// \param[in,out] _in input stream - /// \param[out] _m Matrix4 to read values into - /// \return the stream - public: friend std::istream &operator>>( - std::istream &_in, ignition::math::Matrix4 &_m) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - T d[16]; - _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]; - - 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]); - } - return _in; - } - - /// \brief Get transform which translates to _eye and rotates the X axis - /// so it faces the _target. The rotation is such that Z axis is in the - /// _up direction, if possible. The coordinate system is right-handed, - /// \param[in] _eye Coordinate frame translation. - /// \param[in] _target Point which the X axis should face. If _target is - /// equal to _eye, the X axis won't be rotated. - /// \param[in] _up Direction in which the Z axis should point. If _up is - /// zero or parallel to X, it will be set to +Z. - /// \return Transformation matrix. - public: static Matrix4 LookAt(const Vector3 &_eye, - const Vector3 &_target, const Vector3 &_up = Vector3::UnitZ) - { - // Most important constraint: direction to point X axis at - auto front = _target - _eye; - - // Case when _eye == _target - if (front == Vector3::Zero) - front = Vector3::UnitX; - front.Normalize(); - - // Desired direction to point Z axis at - auto up = _up; - - // Case when _up == Zero - if (up == Vector3::Zero) - up = Vector3::UnitZ; - else - up.Normalize(); - - // Case when _up is parallel to X - if (up.Cross(Vector3::UnitX) == Vector3::Zero) - up = Vector3::UnitZ; - - // Find direction to point Y axis at - auto left = up.Cross(front); - - // Case when front is parallel to up - if (left == Vector3::Zero) - left = Vector3::UnitY; - else - left.Normalize(); - - // Fix up direction so it's perpendicular to XY - up = (front.Cross(left)).Normalize(); - - return Matrix4( - front.X(), left.X(), up.X(), _eye.X(), - front.Y(), left.Y(), up.Y(), _eye.Y(), - front.Z(), left.Z(), up.Z(), _eye.Z(), - 0, 0, 0, 1); - } - - /// \brief The 4x4 matrix - private: T data[4][4]; - }; - - template - const Matrix4 Matrix4::Identity( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1); - - template - const Matrix4 Matrix4::Zero( - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0); - - typedef Matrix4 Matrix4i; - typedef Matrix4 Matrix4d; - typedef Matrix4 Matrix4f; - } - } -} -#endif diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index c8dd33135..319f5c7e8 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -13,581 +13,7 @@ * 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 #include -#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 Identifiers for each of the 4 3x3 corners of the matrix. - public: enum Matrix6Corner - { - /// \brief Top-left corner, consisting of the intersection between the - /// first 3 rows and first 3 columns. - TOP_LEFT = 0, - - /// \brief Top-right corner, consisting of the intersection between the - /// first 3 rows and last 3 columns. - TOP_RIGHT = 1, - - /// \brief Bottom-left corner, consisting of the intersection between - /// the last 3 rows and first 3 columns. - BOTTOM_LEFT = 2, - - /// \brief Bottom-right corner, consisting of the intersection between - /// the last 3 rows and last 3 columns. - BOTTOM_RIGHT = 3 - }; - - /// \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 Set a value in a specific row and col - /// param[in] _row Row of the matrix - /// param[in] _col Col of the matrix - /// param[in] _v Value to assign - /// \return Tru if the value was setted, False otherwise - public: bool SetValue(size_t _row, size_t _col, T _v) - { - if (_row < MatrixSize && _col < MatrixSize) - { - this->data[_row][_col] = _v; - return true; - } - return false; - } - - /// \brief Change the values - /// \param[in] _v00 Row 0, Col 0 value - /// \param[in] _v01 Row 0, Col 1 value - /// \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, - 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 * _m2. - public: Matrix6 operator*=(const Matrix6 &_m2) - { - (*this) = (*this) * _m2; - return *this; - } - - /// \brief Multiplication operator - /// \param[in] _m2 Incoming matrix - /// \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( - 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 Addition assignment operator. This matrix will - /// become equal to this + _m2. - /// \param[in] _m2 Incoming matrix. - /// \return This matrix + _m2. - public: Matrix6 operator+=(const Matrix6 &_m2) - { - (*this) = (*this) + _m2; - return *this; - } - - /// \brief Addition operator - /// \param[in] _m2 Incoming matrix - /// \return This matrix + _m2 - public: Matrix6 operator+(const Matrix6 &_m2) const - { - auto el = [&](size_t _row, size_t _col) -> T - { - return this->data[_row][_col] + _m2(_row, _col); - }; - return Matrix6( - el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5), - el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5), - el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5), - el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5), - el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5), - el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5)); - } - - /// \brief Get the value at the specified row, column index - /// \param[in] _col The column index. Index values are clamped to a - /// range of [0, 5]. - /// \param[in] _row the row index. Index values are clamped to a - /// 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 - { - 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 of the value at the specified row, - /// column index - /// \param[in] _row the row index. Index values are clamped to a - /// range of [0, 5]. - /// \param[in] _col The column index. Index values are clamped to a - /// range of [0, 5]. - /// \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 that - /// 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(Matrix6Corner _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 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] _mat 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. - /// \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 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 - { - 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/include/ignition/math/MovingWindowFilter.hh b/include/ignition/math/MovingWindowFilter.hh index 67bb20c94..42550b5a2 100644 --- a/include/ignition/math/MovingWindowFilter.hh +++ b/include/ignition/math/MovingWindowFilter.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,178 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MOVINGWINDOWFILTER_HH_ -#define IGNITION_MATH_MOVINGWINDOWFILTER_HH_ + */ -#include -#include -#include "ignition/math/Export.hh" - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - - /// \cond - /// \brief Private data members for MovingWindowFilter class. - /// This must be in the header due to templatization. - template< typename T> - class MovingWindowFilterPrivate - { - // \brief Constructor - public: MovingWindowFilterPrivate(); - - /// \brief For moving window smoothed value - public: unsigned int valWindowSize = 4; - - /// \brief buffer history of raw values - public: std::vector valHistory; - - /// \brief iterator pointing to current value in buffer - public: typename std::vector::iterator valIter; - - /// \brief keep track of running sum - public: T sum; - - /// \brief keep track of number of elements - public: unsigned int samples = 0; - }; - - ////////////////////////////////////////////////// - template - MovingWindowFilterPrivate::MovingWindowFilterPrivate() - { - /// \TODO FIXME hardcoded initial value for now - this->valHistory.resize(this->valWindowSize); - this->valIter = this->valHistory.begin(); - this->sum = T(); - } - /// \endcond - - /// \brief Base class for MovingWindowFilter. This replaces the - /// version of MovingWindowFilter in the Ignition Common library. - /// - /// The default window size is 4. - template< typename T> - class MovingWindowFilter - { - /// \brief Constructor - public: MovingWindowFilter(); - - /// \brief Destructor - public: virtual ~MovingWindowFilter(); - - /// \brief Update value of filter - /// \param[in] _val new raw value - public: void Update(const T _val); - - /// \brief Set window size - /// \param[in] _n new desired window size - public: void SetWindowSize(const unsigned int _n); - - /// \brief Get the window size. - /// \return The size of the moving window. - public: unsigned int WindowSize() const; - - /// \brief Get whether the window has been filled. - /// \return True if the window has been filled. - public: bool WindowFilled() const; - - /// \brief Get filtered result - /// \return Latest filtered value - public: T Value() const; - - /// \brief Data pointer. - private: std::unique_ptr> dataPtr; - }; - - ////////////////////////////////////////////////// - template - MovingWindowFilter::MovingWindowFilter() - : dataPtr(new MovingWindowFilterPrivate()) - { - } - - ////////////////////////////////////////////////// - template - MovingWindowFilter::~MovingWindowFilter() - { - this->dataPtr->valHistory.clear(); - } - - ////////////////////////////////////////////////// - template - void MovingWindowFilter::Update(const T _val) - { - // update sum and sample size with incoming _val - - // keep running sum - this->dataPtr->sum += _val; - - // shift pointer, wrap around if end has been reached. - ++this->dataPtr->valIter; - if (this->dataPtr->valIter == this->dataPtr->valHistory.end()) - { - // reset iterator to beginning of queue - this->dataPtr->valIter = this->dataPtr->valHistory.begin(); - } - - // increment sample size - ++this->dataPtr->samples; - - if (this->dataPtr->samples > this->dataPtr->valWindowSize) - { - // subtract old value if buffer already filled - this->dataPtr->sum -= (*this->dataPtr->valIter); - // put new value into queue - (*this->dataPtr->valIter) = _val; - // reduce sample size - --this->dataPtr->samples; - } - else - { - // put new value into queue - (*this->dataPtr->valIter) = _val; - } - } - - ////////////////////////////////////////////////// - template - void MovingWindowFilter::SetWindowSize(const unsigned int _n) - { - this->dataPtr->valWindowSize = _n; - this->dataPtr->valHistory.clear(); - this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize); - this->dataPtr->valIter = this->dataPtr->valHistory.begin(); - this->dataPtr->sum = T(); - this->dataPtr->samples = 0; - } - - ////////////////////////////////////////////////// - template - unsigned int MovingWindowFilter::WindowSize() const - { - return this->dataPtr->valWindowSize; - } - - ////////////////////////////////////////////////// - template - bool MovingWindowFilter::WindowFilled() const - { - return this->dataPtr->samples == this->dataPtr->valWindowSize; - } - - ////////////////////////////////////////////////// - template - T MovingWindowFilter::Value() const - { - return this->dataPtr->sum / static_cast(this->dataPtr->samples); - } - } - } -} -#endif +#include +#include diff --git a/include/ignition/math/OrientedBox.hh b/include/ignition/math/OrientedBox.hh index 64d48b578..5642ecb0e 100644 --- a/include/ignition/math/OrientedBox.hh +++ b/include/ignition/math/OrientedBox.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,274 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_ORIENTEDBOX_HH_ -#define IGNITION_MATH_ORIENTEDBOX_HH_ + */ -#include -#include -#include -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \brief Mathematical representation of a box which can be arbitrarily - /// positioned and rotated. - template - class OrientedBox - { - /// \brief Default constructor - public: OrientedBox() : size(Vector3::Zero), pose(Pose3::Zero) - { - } - - /// \brief Constructor which takes size and pose. - /// \param[in] _size Box size, in its own coordinate frame. Its absolute - /// value will be taken, so the size is non-negative. - /// \param[in] _pose Box pose. - public: OrientedBox(const Vector3 &_size, const Pose3 &_pose) - : size(_size.Abs()), pose(_pose) - { - } - - /// \brief Constructor which takes size, pose, and material. - /// \param[in] _size Box size, in its own coordinate frame. Its absolute - /// value will be taken, so the size is non-negative. - /// \param[in] _pose Box pose. - /// \param[in] _mat Material property for the box. - public: OrientedBox(const Vector3 &_size, const Pose3 &_pose, - const Material &_mat) - : size(_size.Abs()), pose(_pose), material(_mat) - { - } - - /// \brief Constructor which takes only the size. - /// \param[in] _size Box size, in its own coordinate frame. Its absolute - /// value will be taken, so the size is non-negative. - public: explicit OrientedBox(const Vector3 &_size) - : size(_size.Abs()), pose(Pose3::Zero) - { - } - - /// \brief Constructor which takes only the size. - /// \param[in] _size Box size, in its own coordinate frame. Its absolute - /// value will be taken, so the size is non-negative. - /// \param[in] _mat Material property for the box. - public: explicit OrientedBox(const Vector3 &_size, - const Material &_mat) - : size(_size.Abs()), pose(Pose3::Zero), material(_mat) - { - } - - /// \brief Copy constructor. - /// \param[in] _b OrientedBox to copy. - public: OrientedBox(const OrientedBox &_b) - : size(_b.size), pose(_b.pose), material(_b.material) - { - } - - /// \brief Destructor - public: virtual ~OrientedBox() - { - } - - /// \brief Get the length along the x dimension - /// \return Value of the length in the x dimension - public: T XLength() const - { - return this->size.X(); - } - - /// \brief Get the length along the y dimension - /// \return Value of the length in the y dimension - public: T YLength() const - { - return this->size.Y(); - } - - /// \brief Get the length along the z dimension - /// \return Value of the length in the z dimension - public: T ZLength() const - { - return this->size.Z(); - } - - /// \brief Get the size of the box - /// \return Size of the box - public: const Vector3 &Size() const - { - return this->size; - } - - /// \brief Get the box pose, which is the pose of its center. - /// \return The pose of the box. - public: const Pose3 &Pose() const - { - return this->pose; - } - - /// \brief Set the box size. - /// \param[in] _size Box size, in its own coordinate frame. Its absolute - /// value will be taken, so the size is non-negative. - public: void Size(Vector3 &_size) - { - // Enforce non-negative size - this->size = _size.Abs(); - } - - /// \brief Set the box pose. - /// \param[in] _pose Box pose. - public: void Pose(Pose3 &_pose) - { - this->pose = _pose; - } - - /// \brief Assignment operator. Set this box to the parameter - /// \param[in] _b OrientedBox to copy - /// \return The new box. - public: OrientedBox &operator=(const OrientedBox &_b) - { - this->size = _b.size; - this->pose = _b.pose; - this->material = _b.material; - return *this; - } - - /// \brief Equality test operator - /// \param[in] _b OrientedBox to test - /// \return True if equal - public: bool operator==(const OrientedBox &_b) const - { - return this->size == _b.size && this->pose == _b.pose && - this->material == _b.material; - } - - /// \brief Inequality test operator - /// \param[in] _b OrientedBox to test - /// \return True if not equal - public: bool operator!=(const OrientedBox &_b) const - { - return this->size != _b.size || this->pose != _b.pose || - this->material != _b.material; - } - - /// \brief Output operator - /// \param[in] _out Output stream - /// \param[in] _b OrientedBox to output to the stream - /// \return The stream - public: friend std::ostream &operator<<(std::ostream &_out, - const OrientedBox &_b) - { - _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] " - << "Material[" << _b.Material().Name() << "]"; - return _out; - } - - /// \brief Check if a point lies inside the box. - /// \param[in] _p Point to check. - /// \return True if the point is inside the box. - public: bool Contains(const Vector3d &_p) const - { - // Move point to box frame - auto t = Matrix4(this->pose).Inverse(); - auto p = t *_p; - - return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 && - p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 && - p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5; - } - - /// \brief Get the material associated with this box. - /// \return The material assigned to this box. - public: const ignition::math::Material &Material() const - { - return this->material; - } - - /// \brief Set the material associated with this box. - /// \param[in] _mat The material assigned to this box. - public: void SetMaterial(const ignition::math::Material &_mat) - { - this->material = _mat; - } - - /// \brief Get the volume of the box in m^3. - /// \return Volume of the box in m^3. - public: T Volume() const - { - return this->size.X() * this->size.Y() * this->size.Z(); - } - - /// \brief Compute the box's density given a mass value. The - /// box is assumed to be solid with uniform density. This - /// function requires the box's size to be set to - /// values greater than zero. The Material of the box is ignored. - /// \param[in] _mass Mass of the box, in kg. This value should be - /// greater than zero. - /// \return Density of the box in kg/m^3. A negative value is - /// returned if the size or _mass is <= 0. - public: T DensityFromMass(const T _mass) const - { - if (this->size.Min() <= 0|| _mass <= 0) - return -1.0; - - return _mass / this->Volume(); - } - - /// \brief Set the density of this box based on a mass value. - /// Density is computed using - /// double DensityFromMass(const double _mass) const. The - /// box is assumed to be solid with uniform density. This - /// function requires the box's size to be set to - /// values greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the box, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// box's size or the _mass value are <= 0. - /// \sa double DensityFromMass(const double _mass) const - public: bool SetDensityFromMass(const T _mass) - { - T newDensity = this->DensityFromMass(_mass); - if (newDensity > 0) - this->material.SetDensity(newDensity); - return newDensity > 0; - } - - /// \brief Get the mass matrix for this box. This function - /// is only meaningful if the box's size and material - /// have been set. - /// \param[out] _massMat The computed mass matrix will be stored here. - /// \return False if computation of the mass matrix failed, which - /// could be due to an invalid size (<=0) or density (<=0). - public: bool MassMatrix(MassMatrix3 &_massMat) const - { - return _massMat.SetFromBox(this->material, this->size); - } - - /// \brief The size of the box in its local frame. - private: Vector3 size; - - /// \brief The pose of the center of the box. - private: Pose3 pose; - - /// \brief The box's material. - private: ignition::math::Material material; - }; - - typedef OrientedBox OrientedBoxi; - typedef OrientedBox OrientedBoxd; - typedef OrientedBox OrientedBoxf; - } - } -} -#endif diff --git a/include/ignition/math/PID.hh b/include/ignition/math/PID.hh index 3291b3c5b..dba527832 100644 --- a/include/ignition/math/PID.hh +++ b/include/ignition/math/PID.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,222 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_PID_HH_ -#define IGNITION_MATH_PID_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class PID PID.hh ignition/math/PID.hh - /// \brief Generic PID controller class. - /// Generic proportional-integral-derivative controller class that - /// keeps track of PID-error states and control inputs given - /// the state of a system and a user specified target state. - /// It includes a user-adjustable command offset term (feed-forward). - // cppcheck-suppress class_X_Y - class IGNITION_MATH_VISIBLE PID - { - /// \brief Constructor, zeros out Pid values when created and - /// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2]. - /// - /// Disable command clamping by setting _cmdMin to a value larger - /// than _cmdMax. Command clamping is disabled by default. - /// - /// Disable integral clamping by setting _iMin to a value larger - /// than _iMax. Integral clamping is disabled by default. - /// - /// \param[in] _p The proportional gain. - /// \param[in] _i The integral gain. - /// \param[in] _d The derivative gain. - /// \param[in] _imax The integral upper limit. - /// \param[in] _imin The integral lower limit. - /// \param[in] _cmdMax Output max value. - /// \param[in] _cmdMin Output min value. - /// \param[in] _cmdOffset Command offset (feed-forward). - public: PID(const double _p = 0.0, - const double _i = 0.0, - const double _d = 0.0, - const double _imax = -1.0, - const double _imin = 0.0, - const double _cmdMax = -1.0, - const double _cmdMin = 0.0, - const double _cmdOffset = 0.0); - - /// \brief Destructor - public: ~PID() = default; - - /// \brief Initialize PID-gains and integral term - /// limits:[iMax:iMin]-[I1:I2]. - /// - /// Disable command clamping by setting _cmdMin to a value larger - /// than _cmdMax. Command clamping is disabled by default. - /// - /// Disable integral clamping by setting _iMin to a value larger - /// than _iMax. Integral clamping is disabled by default. - /// - /// \param[in] _p The proportional gain. - /// \param[in] _i The integral gain. - /// \param[in] _d The derivative gain. - /// \param[in] _imax The integral upper limit. - /// \param[in] _imin The integral lower limit. - /// \param[in] _cmdMax Output max value. - /// \param[in] _cmdMin Output min value. - /// \param[in] _cmdOffset Command offset (feed-forward). - public: void Init(const double _p = 0.0, - const double _i = 0.0, - const double _d = 0.0, - const double _imax = -1.0, - const double _imin = 0.0, - const double _cmdMax = -1.0, - const double _cmdMin = 0.0, - const double _cmdOffset = 0.0); - - /// \brief Set the proportional Gain. - /// \param[in] _p proportional gain value - public: void SetPGain(const double _p); - - /// \brief Set the integral Gain. - /// \param[in] _i integral gain value - public: void SetIGain(const double _i); - - /// \brief Set the derivative Gain. - /// \param[in] _d derivative gain value - public: void SetDGain(const double _d); - - /// \brief Set the integral upper limit. - /// \param[in] _i integral upper limit value - public: void SetIMax(const double _i); - - /// \brief Set the integral lower limit. - /// \param[in] _i integral lower limit value - public: void SetIMin(const double _i); - - /// \brief Set the maximum value for the command. - /// \param[in] _c The maximum value - public: void SetCmdMax(const double _c); - - /// \brief Set the minimum value for the command. - /// \param[in] _c The minimum value - public: void SetCmdMin(const double _c); - - /// \brief Set the offset value for the command, - /// which is added to the result of the PID controller. - /// \param[in] _c The offset value - public: void SetCmdOffset(const double _c); - - /// \brief Get the proportional Gain. - /// \return The proportional gain value - public: double PGain() const; - - /// \brief Get the integral Gain. - /// \return The integral gain value - public: double IGain() const; - - /// \brief Get the derivative Gain. - /// \return The derivative gain value - public: double DGain() const; - - /// \brief Get the integral upper limit. - /// \return The integral upper limit value - public: double IMax() const; - - /// \brief Get the integral lower limit. - /// \return The integral lower limit value - public: double IMin() const; - - /// \brief Get the maximum value for the command. - /// \return The maximum value - public: double CmdMax() const; - - /// \brief Get the minimun value for the command. - /// \return The maximum value - public: double CmdMin() const; - - /// \brief Get the offset value for the command. - /// \return The offset value - public: double CmdOffset() const; - - /// \brief Update the Pid loop with nonuniform time step size. - /// \param[in] _error Error since last call (p_state - p_target). - /// \param[in] _dt Change in time since last update call. - /// Normally, this is called at every time step, - /// The return value is an updated command to be passed - /// to the object being controlled. - /// \return the command value - public: double Update(const double _error, - const std::chrono::duration &_dt); - - /// \brief Set current target command for this PID controller. - /// \param[in] _cmd New command - public: void SetCmd(const double _cmd); - - /// \brief Return current command for this PID controller. - /// \return the command value - public: double Cmd() const; - - /// \brief Return PID error terms for the controller. - /// \param[in] _pe The proportional error. - /// \param[in] _ie The integral of gain times error. - /// \param[in] _de The derivative error. - public: void Errors(double &_pe, double &_ie, double &_de) const; - - /// \brief Assignment operator - /// \param[in] _p a reference to a PID to assign values from - /// \return reference to this instance - public: PID &operator=(const PID &_p); - - /// \brief Reset the errors and command. - public: void Reset(); - - /// \brief Error at a previous step. - private: double pErrLast = 0.0; - - /// \brief Current error. - private: double pErr = 0.0; - - /// \brief Integral of gain times error. - private: double iErr = 0.0; - - /// \brief Derivative error. - private: double dErr = 0.0; - - /// \brief Gain for proportional control. - private: double pGain; - - /// \brief Gain for integral control. - private: double iGain = 0.0; - - /// \brief Gain for derivative control. - private: double dGain = 0.0; - - /// \brief Maximum clamping value for integral term. - private: double iMax = -1.0; - - /// \brief Minim clamping value for integral term. - private: double iMin = 0.0; - - /// \brief Command value. - private: double cmd = 0.0; - - /// \brief Max command clamping value. - private: double cmdMax = -1.0; - - /// \brief Min command clamping value. - private: double cmdMin = 0.0; - - /// \brief Command offset. - private: double cmdOffset = 0.0; - }; - } - } -} -#endif diff --git a/include/ignition/math/PiecewiseScalarField3.hh b/include/ignition/math/PiecewiseScalarField3.hh index ce938d1bf..c52dd227b 100644 --- a/include/ignition/math/PiecewiseScalarField3.hh +++ b/include/ignition/math/PiecewiseScalarField3.hh @@ -13,207 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_PIECEWISE_SCALAR_FIELD3_HH_ -#define IGNITION_MATH_PIECEWISE_SCALAR_FIELD3_HH_ + */ -#include -#include -#include -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /** \class PiecewiseScalarField3 PiecewiseScalarField3.hh\ - * ignition/math/PiecewiseScalarField3.hh - */ - /// \brief The PiecewiseScalarField3 class constructs a scalar field F - /// in R^3 as a union of scalar fields Pn, defined over regions Rn i.e. - /// piecewise. - /// - /// \tparam ScalarField3T a callable type taking a single Vector3 - /// value as argument and returning a ScalarT value. Additionally: - /// - for PiecewiseScalarField3 to have a stream operator overload, - /// ScalarField3T must support stream operator overload; - /// - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T - /// must implement a - /// ScalarT Minimum(const Region3 &, Vector3 &) - /// method that computes its minimum in the given region and returns - /// an argument value that yields said minimum. - /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits - /// have been specialized. - /// - /// ## Example - /// - /// \snippet examples/piecewise_scalar_field3_example.cc complete - template - class PiecewiseScalarField3 - { - /// \brief A scalar field P in R^3 and - /// the region R in which it is defined - public: struct Piece { - Region3 region; - ScalarField3T field; - }; - - /// \brief Constructor - public: PiecewiseScalarField3() = default; - - /// \brief Constructor - /// \param[in] _pieces scalar fields Pn and the regions Rn - /// in which these are defined. Regions should not overlap. - public: explicit PiecewiseScalarField3(const std::vector &_pieces) - : pieces(_pieces) - { - for (size_t i = 0; i < pieces.size(); ++i) - { - if (pieces[i].region.Empty()) - { - std::cerr << "Region #" << i << " (" << pieces[i].region - << ") in piecewise scalar field definition is empty." - << std::endl; - } - for (size_t j = i + 1; j < pieces.size(); ++j) - { - if (pieces[i].region.Intersects(pieces[j].region)) - { - std::cerr << "Detected overlap between regions in " - << "piecewise scalar field definition: " - << "region #" << i << " (" << pieces[i].region - << ") overlaps with region #" << j << " (" - << pieces[j].region << "). Region #" << i - << " will take precedence when overlapping." - << std::endl; - } - } - } - } - - /// \brief Define piecewise scalar field as `_field` throughout R^3 space - /// \param[in] _field a scalar field in R^3 - /// \return `_field` as a piecewise scalar field - public: static PiecewiseScalarField3 Throughout(ScalarField3T _field) - { - return PiecewiseScalarField3({ - {Region3::Unbounded, std::move(_field)}}); - } - - /// \brief Evaluate the piecewise scalar field at `_p` - /// \param[in] _p piecewise scalar field argument - /// \return the result of evaluating `F(_p)`, or NaN - /// if the scalar field is not defined at `_p` - public: ScalarT Evaluate(const Vector3 &_p) const - { - auto it = std::find_if( - this->pieces.begin(), this->pieces.end(), - [&](const Piece &piece) - { - return piece.region.Contains(_p); - }); - if (it == this->pieces.end()) - { - return std::numeric_limits::quiet_NaN(); - } - return it->field(_p); - } - - /// \brief Call operator overload - /// \see PiecewiseScalarField3::Evaluate() - /// \param[in] _p piecewise scalar field argument - /// \return the result of evaluating `F(_p)`, or NaN - /// if the scalar field is not defined at `_p` - public: ScalarT operator()(const Vector3 &_p) const - { - return this->Evaluate(_p); - } - - /// \brief Compute the piecewise scalar field minimum - /// Note that, since this method computes the minimum - /// for each region independently, it implicitly assumes - /// continuity in the boundaries between regions, if any. - /// \param[out] _pMin scalar field argument that yields - /// the minimum, or NaN if the scalar field is not - /// defined anywhere (i.e. default constructed) - /// \return the scalar field minimum, or NaN if the - /// scalar field is not defined anywhere (i.e. default - /// constructed) - public: ScalarT Minimum(Vector3 &_pMin) const - { - if (this->pieces.empty()) - { - _pMin = Vector3::NaN; - return std::numeric_limits::quiet_NaN(); - } - ScalarT yMin = std::numeric_limits::infinity(); - for (const Piece &piece : this->pieces) - { - if (!piece.region.Empty()) - { - Vector3 p; - const ScalarT y = piece.field.Minimum(piece.region, p); - if (y < yMin) - { - _pMin = p; - yMin = y; - } - } - } - return yMin; - } - - /// \brief Compute the piecewise scalar field minimum - /// \return the scalar field minimum, or NaN if the - /// scalar field is not defined anywhere (i.e. default - /// constructed) - public: ScalarT Minimum() const - { - Vector3 pMin; - return this->Minimum(pMin); - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _field SeparableScalarField3 to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, - const ignition::math::PiecewiseScalarField3< - ScalarField3T, ScalarT> &_field) - { - if (_field.pieces.empty()) - { - return _out << "undefined"; - } - for (size_t i = 0; i < _field.pieces.size() - 1; ++i) - { - _out << _field.pieces[i].field << " if (x, y, z) in " - << _field.pieces[i].region << "; "; - } - return _out << _field.pieces.back().field - << " if (x, y, z) in " - << _field.pieces.back().region; - } - - /// \brief Scalar fields Pn and the regions Rn in which these are defined - private: std::vector pieces; - }; - - template - using PiecewiseScalarField3f = PiecewiseScalarField3; - template - using PiecewiseScalarField3d = PiecewiseScalarField3; - } - } -} - -#endif diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 4994f8128..bf1fcf1a5 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,279 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_PLANE_HH_ -#define IGNITION_MATH_PLANE_HH_ + */ -#include -#include -#include +#include #include -#include -#include -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Plane Plane.hh ignition/math/Plane.hh - /// \brief A plane and related functions. - template - class Plane - { - /// \brief Enum used to indicate a side of the plane, no side, or both - /// sides for entities on the plane. - /// \sa Side - public: enum PlaneSide - { - /// \brief Negative side of the plane. This is the side that is - /// opposite the normal. - NEGATIVE_SIDE = 0, - - /// \brief Positive side of the plane. This is the side that has the - /// normal vector. - POSITIVE_SIDE = 1, - - /// \brief On the plane. - NO_SIDE = 2, - - /// \brief On both sides of the plane. - BOTH_SIDE = 3 - }; - - /// \brief Constructor - public: Plane() - : d(0.0) - { - } - - /// \brief Constructor from a normal and a distance - /// \param[in] _normal The plane normal - /// \param[in] _offset Offset along the normal - public: Plane(const Vector3 &_normal, T _offset = 0.0) - : normal(_normal), d(_offset) - { - } - - /// \brief Constructor - /// \param[in] _normal The plane normal - /// \param[in] _size Size of the plane - /// \param[in] _offset Offset along the normal - public: Plane(const Vector3 &_normal, const Vector2 &_size, - T _offset) - { - this->Set(_normal, _size, _offset); - } - - /// \brief Copy constructor - /// \param[in] _plane Plane to copy - public: Plane(const Plane &_plane) - : normal(_plane.normal), size(_plane.size), d(_plane.d) - {} - - /// \brief Destructor - public: virtual ~Plane() {} - - /// \brief Set the plane - /// \param[in] _normal The plane normal - /// \param[in] _offset Offset along the normal - public: void Set(const Vector3 &_normal, T _offset) - { - this->normal = _normal; - this->d = _offset; - } - - /// \brief Set the plane - /// \param[in] _normal The plane normal - /// \param[in] _size Size of the plane - /// \param[in] _offset Offset along the normal - public: void Set(const Vector3 &_normal, const Vector2 &_size, - T _offset) - { - this->normal = _normal; - this->size = _size; - this->d = _offset; - } - - /// \brief The distance to the plane from the given point. The - /// distance can be negative, which indicates the point is on the - /// negative side of the plane. - /// \param[in] _point 3D point to calculate distance from. - /// \return Distance from the point to the plane. - /// \sa Side - public: T Distance(const Vector3 &_point) const - { - return this->normal.Dot(_point) - this->d; - } - - /// \brief Get the intersection of an infinite line with the plane, - /// given the line's gradient and a point in parametrized space. - /// \param[in] _point A point that lies on the line. - /// \param[in] _gradient The gradient of the line. - /// \param[in] _tolerance The tolerance for determining a line is - /// parallel to the plane. Optional, default=10^-16 - /// \return The point of intersection. std::nullopt if the line is - /// parallel to the plane (including lines on the plane). - public: std::optional> Intersection( - const Vector3 &_point, - const Vector3 &_gradient, - const double &_tolerance = 1e-6) const - { - if (std::abs(this->Normal().Dot(_gradient)) < _tolerance) - { - return std::nullopt; - } - auto constant = this->Offset() - this->Normal().Dot(_point); - auto param = constant / this->Normal().Dot(_gradient); - auto intersection = _point + _gradient*param; - - if (this->Size() == Vector2(0, 0)) - return intersection; - - // Check if the point is within the size bounds - // To do this we create a Quaternion using Angle, Axis constructor and - // rotate the Y and X axis the same amount as the normal. - auto dotProduct = Vector3::UnitZ.Dot(this->Normal()); - auto angle = acos(dotProduct / this->Normal().Length()); - auto axis = Vector3::UnitZ.Cross(this->Normal().Normalized()); - Quaternion rotation(axis, angle); - - Vector3 rotatedXAxis = rotation * Vector3::UnitX; - Vector3 rotatedYAxis = rotation * Vector3::UnitY; - - auto xBasis = rotatedXAxis.Dot(intersection); - auto yBasis = rotatedYAxis.Dot(intersection); - - if (std::abs(xBasis) < this->Size().X() / 2 && - std::abs(yBasis) < this->Size().Y() / 2) - { - return intersection; - } - return std::nullopt; - } - - /// \brief The side of the plane a point is on. - /// \param[in] _point The 3D point to check. - /// \return Plane::NEGATIVE_SIDE if the distance from the point to the - /// plane is negative, Plane::POSITIVE_SIDE if the distance from the - /// point to the plane is positive, or Plane::NO_SIDE if the - /// point is on the plane. - public: PlaneSide Side(const Vector3 &_point) const - { - T dist = this->Distance(_point); - - if (dist < 0.0) - return NEGATIVE_SIDE; - - if (dist > 0.0) - return POSITIVE_SIDE; - - return NO_SIDE; - } - - /// \brief The side of the plane a box is on. - /// \param[in] _box The 3D box to check. - /// \return Plane::NEGATIVE_SIDE if the distance from the box to the - /// plane is negative, Plane::POSITIVE_SIDE if the distance from the - /// box to the plane is positive, or Plane::BOTH_SIDE if the - /// box is on the plane. - public: PlaneSide Side(const math::AxisAlignedBox &_box) const - { - double dist = this->Distance(_box.Center()); - double maxAbsDist = this->normal.AbsDot(_box.Size()/2.0); - - if (dist < -maxAbsDist) - return NEGATIVE_SIDE; - - if (dist > maxAbsDist) - return POSITIVE_SIDE; - - return BOTH_SIDE; - } - - /// \brief Get distance to the plane give an origin and direction - /// \param[in] _origin the origin - /// \param[in] _dir a direction - /// \return the shortest distance - public: T Distance(const Vector3 &_origin, - const Vector3 &_dir) const - { - T denom = this->normal.Dot(_dir); - - if (std::abs(denom) < 1e-3) - { - // parallel - return 0; - } - else - { - T nom = _origin.Dot(this->normal) - this->d; - T t = -(nom/denom); - return t; - } - } - - /// \brief Get the plane size - public: inline const Vector2 &Size() const - { - return this->size; - } - - /// \brief Get the plane size - public: inline Vector2 &Size() - { - return this->size; - } - - /// \brief Get the plane offset - public: inline const Vector3 &Normal() const - { - return this->normal; - } - - /// \brief Get the plane offset - public: inline Vector3 &Normal() - { - return this->normal; - } - - /// \brief Get the plane offset - public: inline T Offset() const - { - return this->d; - } - - /// \brief Equal operator - /// \param _p another plane - /// \return itself - public: Plane &operator=(const Plane &_p) - { - this->normal = _p.normal; - this->size = _p.size; - this->d = _p.d; - - return *this; - } - - /// \brief Plane normal - private: Vector3 normal; - - /// \brief Plane size - private: Vector2 size; - - /// \brief Plane offset - private: T d; - }; - - typedef Plane Planei; - typedef Plane Planed; - typedef Plane Planef; - } - } -} - -#endif diff --git a/include/ignition/math/Polynomial3.hh b/include/ignition/math/Polynomial3.hh index 4112d8f80..9f1238926 100644 --- a/include/ignition/math/Polynomial3.hh +++ b/include/ignition/math/Polynomial3.hh @@ -13,283 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_POLYNOMIAL3_HH_ -#define IGNITION_MATH_POLYNOMIAL3_HH_ + */ -#include -#include -#include -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Polynomial3 Polynomial3.hh ignition/math/Polynomial3.hh - /// \brief The Polynomial3 class represents a cubic polynomial - /// with real coefficients p(x) = c0 x^3 + c1 x^2 + c2 x + c3. - /// ## Example - /// - /// \snippet examples/polynomial3_example.cc complete - template - class Polynomial3 - { - /// \brief Constructor - public: Polynomial3() = default; - - /// \brief Constructor - /// \param[in] _coeffs coefficients c0 through c3, left to right - public: explicit Polynomial3(Vector4 _coeffs) - : coeffs(std::move(_coeffs)) - { - } - - /// \brief Make a constant polynomial - /// \return a p(x) = `_value` polynomial - public: static Polynomial3 Constant(T _value) - { - return Polynomial3(Vector4(0., 0., 0., _value)); - } - - /// \brief Get the polynomial coefficients - /// \return this polynomial coefficients - public: const Vector4 &Coeffs() const { return this->coeffs; } - - /// \brief Evaluate the polynomial at `_x` - /// For non-finite `_x`, this function - /// computes p(z) as z tends to `_x`. - /// \param[in] _x polynomial argument - /// \return the result of evaluating p(`_x`) - public: T Evaluate(const T &_x) const - { - using std::isnan, std::isfinite; - if (isnan(_x)) - { - return _x; - } - if (!isfinite(_x)) - { - using std::abs, std::copysign; - const T epsilon = - std::numeric_limits::epsilon(); - if (abs(this->coeffs[0]) >= epsilon) - { - return _x * copysign(T(1.), this->coeffs[0]); - } - if (abs(this->coeffs[1]) >= epsilon) - { - return copysign(_x, this->coeffs[1]); - } - if (abs(this->coeffs[2]) >= epsilon) - { - return _x * copysign(T(1.), this->coeffs[2]); - } - return this->coeffs[3]; - } - const T _x2 = _x * _x; - const T _x3 = _x2 * _x; - - return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 + - this->coeffs[2] * _x + this->coeffs[3]); - } - - /// \brief Call operator overload - /// \see Polynomial3::Evaluate() - public: T operator()(const T &_x) const - { - return this->Evaluate(_x); - } - - /// \brief Compute polynomial minimum in an `_interval` - /// \param[in] _interval polynomial argument interval to check - /// \param[out] _xMin polynomial argument that yields minimum, - /// or NaN if the interval is empty - /// \return the polynomial minimum in the given interval, - /// or NaN if the interval is empty - public: T Minimum(const Interval &_interval, T &_xMin) const - { - if (_interval.Empty()) - { - _xMin = std::numeric_limits::quiet_NaN(); - return std::numeric_limits::quiet_NaN(); - } - T yMin; - // For open intervals, assume continuity in the limit - const T &xLeft = _interval.LeftValue(); - const T &xRight = _interval.RightValue(); - const T yLeft = this->Evaluate(xLeft); - const T yRight = this->Evaluate(xRight); - if (yLeft <= yRight) - { - yMin = yLeft; - _xMin = xLeft; - } - else - { - yMin = yRight; - _xMin = xRight; - } - using std::abs, std::sqrt; // enable ADL - constexpr T epsilon = std::numeric_limits::epsilon(); - if (abs(this->coeffs[0]) >= epsilon) - { - // Polynomial function p(x) is cubic, look - // for local minima within the given interval - - // Find local extrema by computing the roots - // of p'(x), a quadratic polynomial function - const T a = this->coeffs[0] * T(3.); - const T b = this->coeffs[1] * T(2.); - const T c = this->coeffs[2]; - - const T discriminant = b * b - T(4.) * a * c; - if (discriminant >= T(0.)) - { - // Roots of p'(x) are real, check local minima - const T x = (-b + sqrt(discriminant)) / (T(2.) * a); - if (_interval.Contains(x)) - { - const T y = this->Evaluate(x); - if (y < yMin) - { - _xMin = x; - yMin = y; - } - } - } - } - else if (abs(this->coeffs[1]) >= epsilon) - { - // Polynomial function p(x) is quadratic, - // look for global minima if concave - const T a = this->coeffs[1]; - const T b = this->coeffs[2]; - if (a > T(0.)) - { - const T x = -b / (T(2.) * a); - if (_interval.Contains(x)) - { - const T y = this->Evaluate(x); - if (y < yMin) - { - _xMin = x; - yMin = y; - } - } - } - } - return yMin; - } - - /// \brief Compute polynomial minimum in an `_interval` - /// \param[in] _interval polynomial argument interval to check - /// \return the polynomial minimum in the given interval (may - /// not be finite), or NaN if the interval is empty - public: T Minimum(const Interval &_interval) const - { - T xMin; - return this->Minimum(_interval, xMin); - } - - /// \brief Compute polynomial minimum - /// \param[out] _xMin polynomial argument that yields minimum - /// \return the polynomial minimum (may not be finite) - public: T Minimum(T &_xMin) const - { - return this->Minimum(Interval::Unbounded, _xMin); - } - - /// \brief Compute polynomial minimum - /// \return the polynomial minimum (may not be finite) - public: T Minimum() const - { - T xMin; - return this->Minimum(Interval::Unbounded, xMin); - } - - /// \brief Prints polynomial as p(`_x`) to `_out` stream - /// \param[in] _out Output stream to print to - /// \param[in] _x Argument name to be used - public: void Print(std::ostream &_out, const std::string &_x = "x") const - { - constexpr T epsilon = - std::numeric_limits::epsilon(); - bool streamStarted = false; - for (size_t i = 0; i < 4; ++i) - { - using std::abs; // enable ADL - const T magnitude = abs(this->coeffs[i]); - const bool sign = this->coeffs[i] < T(0); - const int exponent = 3 - i; - if (magnitude >= epsilon) - { - if (streamStarted) - { - if (sign) - { - _out << " - "; - } - else - { - _out << " + "; - } - } - else if (sign) - { - _out << "-"; - } - if (exponent > 0) - { - if ((magnitude - T(1)) > epsilon) - { - _out << magnitude << " "; - } - _out << _x; - if (exponent > 1) - { - _out << "^" << exponent; - } - } - else - { - _out << magnitude; - } - streamStarted = true; - } - } - if (!streamStarted) - { - _out << this->coeffs[3]; - } - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _p Polynomial3 to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Polynomial3 &_p) - { - _p.Print(_out, "x"); - return _out; - } - - /// \brief Polynomial coefficients - private: Vector4 coeffs; - }; - using Polynomial3f = Polynomial3; - using Polynomial3d = Polynomial3; - } - } -} - -#endif diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index 28c86ae30..5981cc5f8 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,488 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_POSE_HH_ -#define IGNITION_MATH_POSE_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Pose3 Pose3.hh ignition/math/Pose3.hh - /// \brief Encapsulates a position and rotation in three space - template - class Pose3 - { - /// \brief math::Pose3(0, 0, 0, 0, 0, 0) - public: static const Pose3 Zero; - - /// \brief Default constructors - public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0) - { - } - - /// \brief Constructor - /// \param[in] _pos A position - /// \param[in] _rot A rotation - public: Pose3(const Vector3 &_pos, const Quaternion &_rot) - : p(_pos), q(_rot) - { - } - - /// \brief Constructor - /// \param[in] _x x position in meters. - /// \param[in] _y y position in meters. - /// \param[in] _z z position in meters. - /// \param[in] _roll Roll (rotation about X-axis) in radians. - /// \param[in] _pitch Pitch (rotation about y-axis) in radians. - /// \param[in] _yaw Yaw (rotation about z-axis) in radians. - public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) - : p(_x, _y, _z), q(_roll, _pitch, _yaw) - { - } - - /// \brief Constructor - /// \param[in] _x x position in meters. - /// \param[in] _y y position in meters. - /// \param[in] _z z position in meters. - /// \param[in] _qw Quaternion w value. - /// \param[in] _qx Quaternion x value. - /// \param[in] _qy Quaternion y value. - /// \param[in] _qz Quaternion z value. - public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz) - : p(_x, _y, _z), q(_qw, _qx, _qy, _qz) - { - } - - /// \brief Copy constructor - /// \param[in] _pose Pose3 to copy - public: Pose3(const Pose3 &_pose) - : p(_pose.p), q(_pose.q) - { - } - - /// \brief Destructor - public: virtual ~Pose3() - { - } - - /// \brief Set the pose from a Vector3 and a Quaternion - /// \param[in] _pos The position. - /// \param[in] _rot The rotation. - public: void Set(const Vector3 &_pos, const Quaternion &_rot) - { - this->p = _pos; - this->q = _rot; - } - - /// \brief Set the pose from pos and rpy vectors - /// \param[in] _pos The position. - /// \param[in] _rpy The rotation expressed as Euler angles. - public: void Set(const Vector3 &_pos, const Vector3 &_rpy) - { - this->p = _pos; - this->q.Euler(_rpy); - } - - /// \brief Set the pose from a six tuple. - /// \param[in] _x x position in meters. - /// \param[in] _y y position in meters. - /// \param[in] _z z position in meters. - /// \param[in] _roll Roll (rotation about X-axis) in radians. - /// \param[in] _pitch Pitch (rotation about y-axis) in radians. - /// \param[in] _yaw Pitch (rotation about z-axis) in radians. - public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw) - { - this->p.Set(_x, _y, _z); - this->q.Euler(math::Vector3(_roll, _pitch, _yaw)); - } - - /// \brief See if a pose is finite (e.g., not nan) - public: bool IsFinite() const - { - return this->p.IsFinite() && this->q.IsFinite(); - } - - /// \brief Fix any nan values - public: inline void Correct() - { - this->p.Correct(); - this->q.Correct(); - } - - /// \brief Get the inverse of this pose - /// \return the inverse pose - public: Pose3 Inverse() const - { - Quaternion inv = this->q.Inverse(); - return Pose3(inv * (this->p*-1), inv); - } - - /// \brief Addition operator - /// A is the transform from O to P specified in frame O - /// B is the transform from P to Q specified in frame P - /// then, B + A is the transform from O to Q specified in frame O - /// \param[in] _pose Pose3 to add to this pose - /// \return The resulting pose - public: Pose3 operator+(const Pose3 &_pose) const - { - Pose3 result; - - result.p = this->CoordPositionAdd(_pose); - result.q = this->CoordRotationAdd(_pose.q); - - return result; - } - - /// \brief Add-Equals operator - /// \param[in] _pose Pose3 to add to this pose - /// \return The resulting pose - public: const Pose3 &operator+=(const Pose3 &_pose) - { - this->p = this->CoordPositionAdd(_pose); - this->q = this->CoordRotationAdd(_pose.q); - - return *this; - } - - /// \brief Negation operator - /// A is the transform from O to P in frame O - /// then -A is transform from P to O specified in frame P - /// \return The resulting pose - public: inline Pose3 operator-() const - { - return Pose3() - *this; - } - - /// \brief Subtraction operator - /// A is the transform from O to P in frame O - /// B is the transform from O to Q in frame O - /// B - A is the transform from P to Q in frame P - /// \param[in] _pose Pose3 to subtract from this one - /// \return The resulting pose - public: inline Pose3 operator-(const Pose3 &_pose) const - { - return Pose3(this->CoordPositionSub(_pose), - this->CoordRotationSub(_pose.q)); - } - - /// \brief Subtraction operator - /// \param[in] _pose Pose3 to subtract from this one - /// \return The resulting pose - public: const Pose3 &operator-=(const Pose3 &_pose) - { - this->p = this->CoordPositionSub(_pose); - this->q = this->CoordRotationSub(_pose.q); - - return *this; - } - - /// \brief Equality operator - /// \param[in] _pose Pose3 for comparison - /// \return True if equal - public: bool operator==(const Pose3 &_pose) const - { - return this->p == _pose.p && this->q == _pose.q; - } - - /// \brief Inequality operator - /// \param[in] _pose Pose3 for comparison - /// \return True if not equal - public: bool operator!=(const Pose3 &_pose) const - { - return this->p != _pose.p || this->q != _pose.q; - } - - /// \brief Multiplication operator. - /// Given X_OP (frame P relative to O) and X_PQ (frame Q relative to P) - /// then X_OQ = X_OP * X_PQ (frame Q relative to O). - /// \param[in] _pose The pose to multiply by. - /// \return The resulting pose. - public: Pose3 operator*(const Pose3 &_pose) const - { - return Pose3(_pose.CoordPositionAdd(*this), this->q * _pose.q); - } - - /// \brief Multiplication assignment operator. This pose will become - /// equal to this * _pose. - /// \param[in] _pose Pose3 to multiply to this pose - /// \return The resulting pose - public: const Pose3 &operator*=(const Pose3 &_pose) - { - *this = *this * _pose; - return *this; - } - - /// \brief Assignment operator - /// \param[in] _pose Pose3 to copy - public: Pose3 &operator=(const Pose3 &_pose) - { - this->p = _pose.p; - this->q = _pose.q; - return *this; - } - - /// \brief Add one point to a vector: result = this + pos - /// \param[in] _pos Position to add to this pose - /// \return the resulting position - public: Vector3 CoordPositionAdd(const Vector3 &_pos) const - { - Quaternion tmp(0.0, _pos.X(), _pos.Y(), _pos.Z()); - - // result = pose.q + pose.q * this->p * pose.q! - tmp = this->q * (tmp * this->q.Inverse()); - - return Vector3(this->p.X() + tmp.X(), - this->p.Y() + tmp.Y(), - this->p.Z() + tmp.Z()); - } - - /// \brief Add one point to another: result = this + pose - /// \param[in] _pose The Pose3 to add - /// \return The resulting position - public: Vector3 CoordPositionAdd(const Pose3 &_pose) const - { - Quaternion tmp(static_cast(0), - this->p.X(), this->p.Y(), this->p.Z()); - - // result = _pose.q + _pose.q * this->p * _pose.q! - tmp = _pose.q * (tmp * _pose.q.Inverse()); - - return Vector3(_pose.p.X() + tmp.X(), - _pose.p.Y() + tmp.Y(), - _pose.p.Z() + tmp.Z()); - } - - /// \brief Subtract one position from another: result = this - pose - /// \param[in] _pose Pose3 to subtract - /// \return The resulting position - public: inline Vector3 CoordPositionSub(const Pose3 &_pose) const - { - Quaternion tmp(0, - this->p.X() - _pose.p.X(), - this->p.Y() - _pose.p.Y(), - this->p.Z() - _pose.p.Z()); - - tmp = _pose.q.Inverse() * (tmp * _pose.q); - return Vector3(tmp.X(), tmp.Y(), tmp.Z()); - } - - /// \brief Add one rotation to another: result = this->q + rot - /// \param[in] _rot Rotation to add - /// \return The resulting rotation - public: Quaternion CoordRotationAdd(const Quaternion &_rot) const - { - return Quaternion(_rot * this->q); - } - - /// \brief Subtract one rotation from another: result = this->q - rot - /// \param[in] _rot The rotation to subtract - /// \return The resulting rotation - public: inline Quaternion CoordRotationSub( - const Quaternion &_rot) const - { - Quaternion result(_rot.Inverse() * this->q); - result.Normalize(); - return result; - } - - /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and - /// this, find a - /// \param[in] _b the other pose - public: Pose3 CoordPoseSolve(const Pose3 &_b) const - { - Quaternion qt; - Pose3 a; - - a.q = this->q.Inverse() * _b.q; - qt = a.q * Quaternion(0, this->p.X(), this->p.Y(), this->p.Z()); - qt = qt * a.q.Inverse(); - a.p = _b.p - Vector3(qt.X(), qt.Y(), qt.Z()); - - return a; - } - - /// \brief Reset the pose - public: void Reset() - { - // set the position to zero - this->p.Set(); - this->q = Quaternion::Identity; - } - - /// \brief Rotate vector part of a pose about the origin - /// \param[in] _q rotation - /// \return the rotated pose - public: Pose3 RotatePositionAboutOrigin(const Quaternion &_q) const - { - Pose3 a = *this; - a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X() - +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y() - +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z()); - a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X() - +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y() - +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z()); - a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X() - +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y() - +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z()); - return a; - } - - /// \brief Round all values to _precision decimal places - /// \param[in] _precision - public: void Round(int _precision) - { - this->q.Round(_precision); - this->p.Round(_precision); - } - - /// \brief Get the position. - /// \return Origin of the pose. - public: inline const Vector3 &Pos() const - { - return this->p; - } - - /// \brief Get a mutable reference to the position. - /// \return Origin of the pose. - public: inline Vector3 &Pos() - { - return this->p; - } - - /// \brief Get the X value of the position. - /// \return Value X of the origin of the pose. - /// \note The return is made by value since - /// Vector3.X() is already a reference. - public: inline const T X() const - { - return this->p.X(); - } - - /// \brief Set X value of the position. - public: inline void SetX(T x) - { - this->p.X() = x; - } - - /// \brief Get the Y value of the position. - /// \return Value Y of the origin of the pose. - /// \note The return is made by value since - /// Vector3.Y() is already a reference. - public: inline const T Y() const - { - return this->p.Y(); - } - - /// \brief Set the Y value of the position. - public: inline void SetY(T y) - { - this->p.Y() = y; - } - - /// \brief Get the Z value of the position. - /// \return Value Z of the origin of the pose. - /// \note The return is made by value since - /// Vector3.Z() is already a reference. - public: inline const T Z() const - { - return this->p.Z(); - } - - /// \brief Set the Z value of the position. - public: inline void SetZ(T z) - { - this->p.Z() = z; - } - - /// \brief Get the rotation. - /// \return Quaternion representation of the rotation. - public: inline const Quaternion &Rot() const - { - return this->q; - } - - /// \brief Get a mutable reference to the rotation. - /// \return Quaternion representation of the rotation. - public: inline Quaternion &Rot() - { - return this->q; - } - - /// \brief Get the Roll value of the rotation. - /// \return Roll value of the orientation. - /// \note The return is made by value since - /// Quaternion.Roll() is already a reference. - public: inline const T Roll() const - { - return this->q.Roll(); - } - - /// \brief Get the Pitch value of the rotation. - /// \return Pitch value of the orientation. - /// \note The return is made by value since - /// Quaternion.Pitch() is already a reference. - public: inline const T Pitch() const - { - return this->q.Pitch(); - } - - /// \brief Get the Yaw value of the rotation. - /// \return Yaw value of the orientation. - /// \note The return is made by value since - /// Quaternion.Yaw() is already a reference. - public: inline const T Yaw() const - { - return this->q.Yaw(); - } - - /// \brief Stream insertion operator - /// \param[in] _out output stream - /// \param[in] _pose pose to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Pose3 &_pose) - { - _out << _pose.Pos() << " " << _pose.Rot(); - return _out; - } - - /// \brief Stream extraction operator - /// \param[in] _in the input stream - /// \param[in] _pose the pose - /// \return the stream - public: friend std::istream &operator>>( - std::istream &_in, ignition::math::Pose3 &_pose) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - Vector3 pos; - Quaternion rot; - _in >> pos >> rot; - _pose.Set(pos, rot); - return _in; - } - - /// \brief The position - private: Vector3 p; - - /// \brief The rotation - private: Quaternion q; - }; - template const Pose3 Pose3::Zero(0, 0, 0, 0, 0, 0); - - typedef Pose3 Pose3i; - typedef Pose3 Pose3d; - typedef Pose3 Pose3f; - } - } -} -#endif diff --git a/include/ignition/math/Quaternion.hh b/include/ignition/math/Quaternion.hh index d8196a087..dc9900eec 100644 --- a/include/ignition/math/Quaternion.hh +++ b/include/ignition/math/Quaternion.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,1095 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_QUATERNION_HH_ -#define IGNITION_MATH_QUATERNION_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - template class Matrix3; - - /// \class Quaternion Quaternion.hh ignition/math/Quaternion.hh - /// \brief A quaternion class - template - class Quaternion - { - /// \brief math::Quaternion(1, 0, 0, 0) - public: static const Quaternion Identity; - - /// \brief math::Quaternion(0, 0, 0, 0) - public: static const Quaternion Zero; - - /// \brief Default Constructor - public: Quaternion() - : qw(1), qx(0), qy(0), qz(0) - { - // quaternion not normalized, because that breaks - // Pose::CoordPositionAdd(...) - } - - /// \brief Constructor - /// \param[in] _w W param - /// \param[in] _x X param - /// \param[in] _y Y param - /// \param[in] _z Z param - public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z) - : qw(_w), qx(_x), qy(_y), qz(_z) - {} - - /// \brief Constructor from Euler angles in radians - /// \param[in] _roll roll - /// \param[in] _pitch pitch - /// \param[in] _yaw yaw - public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw) - { - this->Euler(Vector3(_roll, _pitch, _yaw)); - } - - /// \brief Constructor from axis angle - /// \param[in] _axis the rotation axis - /// \param[in] _angle the rotation angle in radians - public: Quaternion(const Vector3 &_axis, const T &_angle) - { - this->Axis(_axis, _angle); - } - - /// \brief Constructor - /// \param[in] _rpy euler angles, in radians - public: explicit Quaternion(const Vector3 &_rpy) - { - this->Euler(_rpy); - } - - /// \brief Construct from rotation matrix. - /// \param[in] _mat rotation matrix (must be orthogonal, the function - /// doesn't check it) - public: explicit Quaternion(const Matrix3 &_mat) - { - this->Matrix(_mat); - } - - /// \brief Copy constructor - /// \param[in] _qt Quaternion to copy - public: Quaternion(const Quaternion &_qt) - { - this->qw = _qt.qw; - this->qx = _qt.qx; - this->qy = _qt.qy; - this->qz = _qt.qz; - } - - /// \brief Destructor - public: ~Quaternion() {} - - /// \brief Assignment operator - /// \param[in] _qt Quaternion to copy - public: Quaternion &operator=(const Quaternion &_qt) - { - this->qw = _qt.qw; - this->qx = _qt.qx; - this->qy = _qt.qy; - this->qz = _qt.qz; - - return *this; - } - - /// \brief Invert the quaternion - public: void Invert() - { - this->Normalize(); - // this->qw = this->qw; - this->qx = -this->qx; - this->qy = -this->qy; - this->qz = -this->qz; - } - - /// \brief Get the inverse of this quaternion - /// \return Inverse quaternion - public: inline Quaternion Inverse() const - { - T s = 0; - Quaternion q(this->qw, this->qx, this->qy, this->qz); - - // use s to test if quaternion is valid - s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz; - - if (equal(s, static_cast(0))) - { - q.qw = 1.0; - q.qx = 0.0; - q.qy = 0.0; - q.qz = 0.0; - } - else - { - // deal with non-normalized quaternion - // div by s so q * qinv = identity - q.qw = q.qw / s; - q.qx = -q.qx / s; - q.qy = -q.qy / s; - q.qz = -q.qz / s; - } - return q; - } - - /// \brief Return the logarithm - /// \return the log - public: Quaternion Log() const - { - // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length, - // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) = - // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1. - - Quaternion result; - result.qw = 0.0; - - if (std::abs(this->qw) < 1.0) - { - T fAngle = acos(this->qw); - T fSin = sin(fAngle); - if (std::abs(fSin) >= 1e-3) - { - T fCoeff = fAngle/fSin; - result.qx = fCoeff*this->qx; - result.qy = fCoeff*this->qy; - result.qz = fCoeff*this->qz; - return result; - } - } - - result.qx = this->qx; - result.qy = this->qy; - result.qz = this->qz; - - return result; - } - - /// \brief Return the exponent - /// \return the exp - public: Quaternion Exp() const - { - // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then - // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero, - // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1. - - T fAngle = sqrt(this->qx*this->qx+ - this->qy*this->qy+this->qz*this->qz); - T fSin = sin(fAngle); - - Quaternion result; - result.qw = cos(fAngle); - - if (std::abs(fSin) >= 1e-3) - { - T fCoeff = fSin/fAngle; - result.qx = fCoeff*this->qx; - result.qy = fCoeff*this->qy; - result.qz = fCoeff*this->qz; - } - else - { - result.qx = this->qx; - result.qy = this->qy; - result.qz = this->qz; - } - - return result; - } - - /// \brief Normalize the quaternion - public: void Normalize() - { - T s = 0; - - s = T(sqrt(this->qw * this->qw + this->qx * this->qx + - this->qy * this->qy + this->qz * this->qz)); - - if (equal(s, static_cast(0))) - { - this->qw = T(1.0); - this->qx = T(0.0); - this->qy = T(0.0); - this->qz = T(0.0); - } - else - { - this->qw /= s; - this->qx /= s; - this->qy /= s; - this->qz /= s; - } - } - - /// \brief Gets a normalized version of this quaternion - /// \return a normalized quaternion - public: Quaternion Normalized() const - { - Quaternion result = *this; - result.Normalize(); - return result; - } - - /// \brief Set the quaternion from an axis and angle - /// \param[in] _ax X axis - /// \param[in] _ay Y axis - /// \param[in] _az Z axis - /// \param[in] _aa Angle in radians - public: void Axis(T _ax, T _ay, T _az, T _aa) - { - T l; - - l = _ax * _ax + _ay * _ay + _az * _az; - - if (equal(l, static_cast(0))) - { - this->qw = 1; - this->qx = 0; - this->qy = 0; - this->qz = 0; - } - else - { - _aa *= 0.5; - l = sin(_aa) / sqrt(l); - this->qw = cos(_aa); - this->qx = _ax * l; - this->qy = _ay * l; - this->qz = _az * l; - } - - this->Normalize(); - } - - /// \brief Set the quaternion from an axis and angle - /// \param[in] _axis Axis - /// \param[in] _a Angle in radians - public: void Axis(const Vector3 &_axis, T _a) - { - this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a); - } - - /// \brief Set this quaternion from 4 floating numbers - /// \param[in] _w w - /// \param[in] _x x - /// \param[in] _y y - /// \param[in] _z z - public: void Set(T _w, T _x, T _y, T _z) - { - this->qw = _w; - this->qx = _x; - this->qy = _y; - this->qz = _z; - } - - /// \brief Set the quaternion from Euler angles. The order of operations - /// is roll, pitch, yaw around a fixed body frame axis - /// (the original frame of the object before rotation is applied). - /// Roll is a rotation about x, pitch is about y, yaw is about z. - /// \param[in] _vec Euler angle - public: void Euler(const Vector3 &_vec) - { - this->Euler(_vec.X(), _vec.Y(), _vec.Z()); - } - - /// \brief Set the quaternion from Euler angles. - /// \param[in] _roll Roll angle (radians). - /// \param[in] _pitch Pitch angle (radians). - /// \param[in] _yaw Yaw angle (radians). - public: void Euler(T _roll, T _pitch, T _yaw) - { - T phi, the, psi; - - phi = _roll / T(2.0); - the = _pitch / T(2.0); - psi = _yaw / T(2.0); - - this->qw = T(cos(phi) * cos(the) * cos(psi) + - sin(phi) * sin(the) * sin(psi)); - this->qx = T(sin(phi) * cos(the) * cos(psi) - - cos(phi) * sin(the) * sin(psi)); - this->qy = T(cos(phi) * sin(the) * cos(psi) + - sin(phi) * cos(the) * sin(psi)); - this->qz = T(cos(phi) * cos(the) * sin(psi) - - sin(phi) * sin(the) * cos(psi)); - - this->Normalize(); - } - - /// \brief Return the rotation in Euler angles - /// \return This quaternion as an Euler vector - public: Vector3 Euler() const - { - Vector3 vec; - - T tol = static_cast(1e-15); - - Quaternion copy = *this; - T squ; - T sqx; - T sqy; - T sqz; - - copy.Normalize(); - - squ = copy.qw * copy.qw; - sqx = copy.qx * copy.qx; - sqy = copy.qy * copy.qy; - sqz = copy.qz * copy.qz; - - // Pitch - T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy); - if (sarg <= T(-1.0)) - { - vec.Y(T(-0.5*IGN_PI)); - } - else if (sarg >= T(1.0)) - { - vec.Y(T(0.5*IGN_PI)); - } - else - { - vec.Y(T(asin(sarg))); - } - - // If the pitch angle is PI/2 or -PI/2, we can only compute - // the sum roll + yaw. However, any combination that gives - // the right sum will produce the correct orientation, so we - // set yaw = 0 and compute roll. - // pitch angle is PI/2 - if (std::abs(sarg - 1) < tol) - { - vec.Z(0); - vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw), - squ - sqx + sqy - sqz))); - } - // pitch angle is -PI/2 - else if (std::abs(sarg + 1) < tol) - { - vec.Z(0); - vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw), - squ - sqx + sqy - sqz))); - } - else - { - // Roll - vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx), - squ - sqx - sqy + sqz))); - - // Yaw - vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz), - squ + sqx - sqy - sqz))); - } - - return vec; - } - - /// \brief Convert euler angles to quatern. - /// \param[in] _vec The vector of angles to convert. - /// \return The converted quaternion. - public: static Quaternion EulerToQuaternion(const Vector3 &_vec) - { - Quaternion result; - result.Euler(_vec); - return result; - } - - /// \brief Convert euler angles to quatern. - /// \param[in] _x rotation along x - /// \param[in] _y rotation along y - /// \param[in] _z rotation along z - /// \return The converted quaternion. - public: static Quaternion EulerToQuaternion(T _x, T _y, T _z) - { - return EulerToQuaternion(Vector3(_x, _y, _z)); - } - - /// \brief Get the Euler roll angle in radians - /// \return the roll component - public: T Roll() const - { - return this->Euler().X(); - } - - /// \brief Get the Euler pitch angle in radians - /// \return the pitch component - public: T Pitch() const - { - return this->Euler().Y(); - } - - /// \brief Get the Euler yaw angle in radians - /// \return the yaw component - public: T Yaw() const - { - return this->Euler().Z(); - } - - /// \brief Return rotation as axis and angle - /// \param[out] _axis rotation axis - /// \param[out] _angle ccw angle in radians - public: void ToAxis(Vector3 &_axis, T &_angle) const - { - T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz; - if (equal(len, static_cast(0))) - { - _angle = 0.0; - _axis.Set(1, 0, 0); - } - else - { - _angle = 2.0 * acos(this->qw); - T invLen = 1.0 / sqrt(len); - _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen); - } - } - - /// \brief Set from a rotation matrix. - /// \param[in] _mat rotation matrix (must be orthogonal, the function - /// doesn't check it) - /// - /// Implementation inspired by - /// http://www.euclideanspace.com/maths/geometry/rotations/ - /// conversions/matrixToQuaternion/ - void Matrix(const Matrix3 &_mat) - { - const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2); - if (trace > 0.0000001) - { - qw = sqrt(1 + trace) / 2; - const T s = 1.0 / (4 * qw); - qx = (_mat(2, 1) - _mat(1, 2)) * s; - qy = (_mat(0, 2) - _mat(2, 0)) * s; - qz = (_mat(1, 0) - _mat(0, 1)) * s; - } - else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2)) - { - qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2; - const T s = 1.0 / (4 * qx); - qw = (_mat(2, 1) - _mat(1, 2)) * s; - qy = (_mat(1, 0) + _mat(0, 1)) * s; - qz = (_mat(0, 2) + _mat(2, 0)) * s; - } - else if (_mat(1, 1) > _mat(2, 2)) - { - qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2; - const T s = 1.0 / (4 * qy); - qw = (_mat(0, 2) - _mat(2, 0)) * s; - qx = (_mat(0, 1) + _mat(1, 0)) * s; - qz = (_mat(1, 2) + _mat(2, 1)) * s; - } - else - { - qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2; - const T s = 1.0 / (4 * qz); - qw = (_mat(1, 0) - _mat(0, 1)) * s; - qx = (_mat(0, 2) + _mat(2, 0)) * s; - qy = (_mat(1, 2) + _mat(2, 1)) * s; - } - } - - /// \brief Set this quaternion 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 - /// - /// Implementation inspired by - /// http://stackoverflow.com/a/11741520/1076564 - public: void From2Axes(const Vector3 &_v1, const Vector3 &_v2) - { - // generally, we utilize the fact that a quat (w, x, y, z) represents - // rotation of angle 2*w about axis (x, y, z) - // - // so we want to take get a vector half-way between no rotation and the - // double rotation, which is - // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2 - // if _v1 and _v2 are unit quaternions - // - // since we normalize the result anyway, we can omit the division, - // getting the result: - // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized() - // - // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2) - // is multiplied by k = norm(_v1)*norm(_v2) - - const T kCosTheta = _v1.Dot(_v2); - const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength()); - - if (fabs(kCosTheta/k + 1) < 1e-6) - { - // the vectors are opposite - // any vector orthogonal to _v1 - Vector3 other; - { - const Vector3 _v1Abs(_v1.Abs()); - if (_v1Abs.X() < _v1Abs.Y()) - { - if (_v1Abs.X() < _v1Abs.Z()) - { - other.Set(1, 0, 0); - } - else - { - other.Set(0, 0, 1); - } - } - else - { - if (_v1Abs.Y() < _v1Abs.Z()) - { - other.Set(0, 1, 0); - } - else - { - other.Set(0, 0, 1); - } - } - } - - const Vector3 axis(_v1.Cross(other).Normalize()); - - qw = 0; - qx = axis.X(); - qy = axis.Y(); - qz = axis.Z(); - } - else - { - // the vectors are in general position - const Vector3 axis(_v1.Cross(_v2)); - qw = kCosTheta + k; - qx = axis.X(); - qy = axis.Y(); - qz = axis.Z(); - this->Normalize(); - } - } - - /// \brief Scale a Quaternion - /// \param[in] _scale Amount to scale this rotation - public: void Scale(T _scale) - { - Quaternion b; - Vector3 axis; - T angle; - - // Convert to axis-and-angle - this->ToAxis(axis, angle); - angle *= _scale; - - this->Axis(axis.X(), axis.Y(), axis.Z(), angle); - } - - /// \brief Addition operator - /// \param[in] _qt quaternion for addition - /// \return this quaternion + _qt - public: Quaternion operator+(const Quaternion &_qt) const - { - Quaternion result(this->qw + _qt.qw, this->qx + _qt.qx, - this->qy + _qt.qy, this->qz + _qt.qz); - return result; - } - - /// \brief Addition operator - /// \param[in] _qt quaternion for addition - /// \return this quaternion + qt - public: Quaternion operator+=(const Quaternion &_qt) - { - *this = *this + _qt; - - return *this; - } - - /// \brief Subtraction operator - /// \param[in] _qt quaternion to subtract - /// \return this quaternion - _qt - public: Quaternion operator-(const Quaternion &_qt) const - { - Quaternion result(this->qw - _qt.qw, this->qx - _qt.qx, - this->qy - _qt.qy, this->qz - _qt.qz); - return result; - } - - /// \brief Subtraction operator - /// \param[in] _qt Quaternion for subtraction - /// \return This quaternion - qt - public: Quaternion operator-=(const Quaternion &_qt) - { - *this = *this - _qt; - return *this; - } - - /// \brief Multiplication operator - /// \param[in] _q Quaternion for multiplication - /// \return This quaternion multiplied by the parameter - public: inline Quaternion operator*(const Quaternion &_q) const - { - return Quaternion( - this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz, - this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy, - this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx, - this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw); - } - - /// \brief Multiplication operator by a scalar. - /// \param[in] _f factor - /// \return quaternion multiplied by the scalar - public: Quaternion operator*(const T &_f) const - { - return Quaternion(this->qw*_f, this->qx*_f, - this->qy*_f, this->qz*_f); - } - - /// \brief Multiplication operator - /// \param[in] _qt Quaternion for multiplication - /// \return This quaternion multiplied by the parameter - public: Quaternion operator*=(const Quaternion &_qt) - { - *this = *this * _qt; - return *this; - } - - /// \brief Vector3 multiplication operator - /// \param[in] _v vector to multiply - /// \return The result of the vector multiplication - public: Vector3 operator*(const Vector3 &_v) const - { - Vector3 uv, uuv; - Vector3 qvec(this->qx, this->qy, this->qz); - uv = qvec.Cross(_v); - uuv = qvec.Cross(uv); - uv *= (2.0f * this->qw); - uuv *= 2.0f; - - return _v + uv + uuv; - } - - /// \brief Equality test with tolerance. - /// \param[in] _qt Quaternion for comparison - /// \param[in] _tol equality tolerance - /// \return true if the elements of the quaternions are equal - /// within the tolerance specified by _tol, false otherwise - public: bool Equal(const Quaternion &_qt, const T &_tol) const - { - return equal(this->qx, _qt.qx, _tol) && - equal(this->qy, _qt.qy, _tol) && - equal(this->qz, _qt.qz, _tol) && - equal(this->qw, _qt.qw, _tol); - } - - /// \brief Equal to operator - /// \param[in] _qt Quaternion for comparison - /// \return True if equal - public: bool operator==(const Quaternion &_qt) const - { - return this->Equal(_qt, static_cast(0.001)); - } - - /// \brief Not equal to operator - /// \param[in] _qt Quaternion for comparison - /// \return True if not equal - public: bool operator!=(const Quaternion &_qt) const - { - return !(*this == _qt); - } - - /// \brief Unary minus operator - /// \return negates each component of the quaternion - public: Quaternion operator-() const - { - return Quaternion(-this->qw, -this->qx, -this->qy, -this->qz); - } - - /// \brief Rotate a vector using the quaternion - /// \param[in] _vec vector to rotate - /// \return the rotated vector - public: inline Vector3 RotateVector(const Vector3 &_vec) const - { - Quaternion tmp(static_cast(0), - _vec.X(), _vec.Y(), _vec.Z()); - tmp = (*this) * (tmp * this->Inverse()); - return Vector3(tmp.qx, tmp.qy, tmp.qz); - } - - /// \brief Do the reverse rotation of a vector by this quaternion - /// \param[in] _vec the vector - /// \return the reversed vector - public: Vector3 RotateVectorReverse(const Vector3 &_vec) const - { - Quaternion tmp(0.0, _vec.X(), _vec.Y(), _vec.Z()); - - tmp = this->Inverse() * (tmp * (*this)); - - return Vector3(tmp.qx, tmp.qy, tmp.qz); - } - - /// \brief See if a quaternion is finite (e.g., not nan) - /// \return True if quaternion is finite - public: bool IsFinite() const - { - // std::isfinite works with floating point values, need to explicit - // cast to avoid ambiguity in vc++. - return std::isfinite(static_cast(this->qw)) && - std::isfinite(static_cast(this->qx)) && - std::isfinite(static_cast(this->qy)) && - std::isfinite(static_cast(this->qz)); - } - - /// \brief Correct any nan values in this quaternion - public: inline void Correct() - { - // std::isfinite works with floating point values, need to explicit - // cast to avoid ambiguity in vc++. - if (!std::isfinite(static_cast(this->qx))) - this->qx = 0; - if (!std::isfinite(static_cast(this->qy))) - this->qy = 0; - if (!std::isfinite(static_cast(this->qz))) - this->qz = 0; - if (!std::isfinite(static_cast(this->qw))) - this->qw = 1; - - if (equal(this->qw, static_cast(0)) && - equal(this->qx, static_cast(0)) && - equal(this->qy, static_cast(0)) && - equal(this->qz, static_cast(0))) - { - this->qw = 1; - } - } - - /// \brief Return the X axis - /// \return the X axis of the vector - public: Vector3 XAxis() const - { - T fTy = 2.0f*this->qy; - T fTz = 2.0f*this->qz; - - T fTwy = fTy*this->qw; - T fTwz = fTz*this->qw; - T fTxy = fTy*this->qx; - T fTxz = fTz*this->qx; - T fTyy = fTy*this->qy; - T fTzz = fTz*this->qz; - - return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy); - } - - /// \brief Return the Y axis - /// \return the Y axis of the vector - public: Vector3 YAxis() const - { - T fTx = 2.0f*this->qx; - T fTy = 2.0f*this->qy; - T fTz = 2.0f*this->qz; - T fTwx = fTx*this->qw; - T fTwz = fTz*this->qw; - T fTxx = fTx*this->qx; - T fTxy = fTy*this->qx; - T fTyz = fTz*this->qy; - T fTzz = fTz*this->qz; - - return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx); - } - - /// \brief Return the Z axis - /// \return the Z axis of the vector - public: Vector3 ZAxis() const - { - T fTx = 2.0f*this->qx; - T fTy = 2.0f*this->qy; - T fTz = 2.0f*this->qz; - T fTwx = fTx*this->qw; - T fTwy = fTy*this->qw; - T fTxx = fTx*this->qx; - T fTxz = fTz*this->qx; - T fTyy = fTy*this->qy; - T fTyz = fTz*this->qy; - - return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy)); - } - - /// \brief Round all values to _precision decimal places - /// \param[in] _precision the precision - public: void Round(int _precision) - { - this->qx = precision(this->qx, _precision); - this->qy = precision(this->qy, _precision); - this->qz = precision(this->qz, _precision); - this->qw = precision(this->qw, _precision); - } - - /// \brief Dot product - /// \param[in] _q the other quaternion - /// \return the product - public: T Dot(const Quaternion &_q) const - { - return this->qw*_q.qw + this->qx * _q.qx + - this->qy*_q.qy + this->qz*_q.qz; - } - - /// \brief Spherical quadratic interpolation - /// given the ends and an interpolation parameter between 0 and 1 - /// \param[in] _fT the interpolation parameter - /// \param[in] _rkP the beginning quaternion - /// \param[in] _rkA first intermediate quaternion - /// \param[in] _rkB second intermediate quaternion - /// \param[in] _rkQ the end quaternion - /// \param[in] _shortestPath when true, the rotation may be inverted to - /// get to minimize rotation - /// \return The result of the quadratic interpolation - public: static Quaternion Squad(T _fT, - const Quaternion &_rkP, const Quaternion &_rkA, - const Quaternion &_rkB, const Quaternion &_rkQ, - bool _shortestPath = false) - { - T fSlerpT = 2.0f*_fT*(1.0f-_fT); - Quaternion kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath); - Quaternion kSlerpQ = Slerp(_fT, _rkA, _rkB); - return Slerp(fSlerpT, kSlerpP, kSlerpQ); - } - - /// \brief Spherical linear interpolation between 2 quaternions, - /// given the ends and an interpolation parameter between 0 and 1 - /// \param[in] _fT the interpolation parameter - /// \param[in] _rkP the beginning quaternion - /// \param[in] _rkQ the end quaternion - /// \param[in] _shortestPath when true, the rotation may be inverted to - /// get to minimize rotation - /// \return The result of the linear interpolation - public: static Quaternion Slerp(T _fT, - const Quaternion &_rkP, const Quaternion &_rkQ, - bool _shortestPath = false) - { - T fCos = _rkP.Dot(_rkQ); - Quaternion rkT; - - // Do we need to invert rotation? - if (fCos < 0.0f && _shortestPath) - { - fCos = -fCos; - rkT = -_rkQ; - } - else - { - rkT = _rkQ; - } - - if (std::abs(fCos) < 1 - 1e-03) - { - // Standard case (slerp) - T fSin = sqrt(1 - (fCos*fCos)); - T fAngle = atan2(fSin, fCos); - // FIXME: should check if (std::abs(fSin) >= 1e-3) - T fInvSin = 1.0f / fSin; - T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin; - T fCoeff1 = sin(_fT * fAngle) * fInvSin; - return _rkP * fCoeff0 + rkT * fCoeff1; - } - else - { - // There are two situations: - // 1. "rkP" and "rkQ" are very close (fCos ~= +1), - // so we can do a linear interpolation safely. - // 2. "rkP" and "rkQ" are almost inverse of each - // other (fCos ~= -1), there - // are an infinite number of possibilities interpolation. - // but we haven't have method to fix this case, so just use - // linear interpolation here. - Quaternion t = _rkP * (1.0f - _fT) + rkT * _fT; - // taking the complement requires renormalisation - t.Normalize(); - return t; - } - } - - /// \brief Integrate quaternion for constant angular velocity vector - /// along specified interval `_deltaT`. - /// Implementation based on: - /// http://physicsforgames.blogspot.com/2010/02/quaternions.html - /// \param[in] _angularVelocity Angular velocity vector, specified in - /// same reference frame as base of this quaternion. - /// \param[in] _deltaT Time interval in seconds to integrate over. - /// \return Quaternion at integrated configuration. - public: Quaternion Integrate(const Vector3 &_angularVelocity, - const T _deltaT) const - { - Quaternion deltaQ; - Vector3 theta = _angularVelocity * _deltaT / 2; - T thetaMagSq = theta.SquaredLength(); - T s; - if (thetaMagSq * thetaMagSq / 24.0 < MIN_D) - { - deltaQ.W() = 1.0 - thetaMagSq / 2.0; - s = 1.0 - thetaMagSq / 6.0; - } - else - { - double thetaMag = sqrt(thetaMagSq); - deltaQ.W() = cos(thetaMag); - s = sin(thetaMag) / thetaMag; - } - deltaQ.X() = theta.X() * s; - deltaQ.Y() = theta.Y() * s; - deltaQ.Z() = theta.Z() * s; - return deltaQ * (*this); - } - - /// \brief Get the w component. - /// \return The w quaternion component. - public: inline const T &W() const - { - return this->qw; - } - - /// \brief Get the x component. - /// \return The x quaternion component. - public: inline const T &X() const - { - return this->qx; - } - - /// \brief Get the y component. - /// \return The y quaternion component. - public: inline const T &Y() const - { - return this->qy; - } - - /// \brief Get the z component. - /// \return The z quaternion component. - public: inline const T &Z() const - { - return this->qz; - } - - - /// \brief Get a mutable w component. - /// \return The w quaternion component. - public: inline T &W() - { - return this->qw; - } - - /// \brief Get a mutable x component. - /// \return The x quaternion component. - public: inline T &X() - { - return this->qx; - } - - /// \brief Get a mutable y component. - /// \return The y quaternion component. - public: inline T &Y() - { - return this->qy; - } - - /// \brief Get a mutable z component. - /// \return The z quaternion component. - public: inline T &Z() - { - return this->qz; - } - - /// \brief Set the x component. - /// \param[in] _v The new value for the x quaternion component. - public: inline void X(T _v) - { - this->qx = _v; - } - - /// \brief Set the y component. - /// \param[in] _v The new value for the y quaternion component. - public: inline void Y(T _v) - { - this->qy = _v; - } - - /// \brief Set the z component. - /// \param[in] _v The new value for the z quaternion component. - public: inline void Z(T _v) - { - this->qz = _v; - } - - /// \brief Set the w component. - /// \param[in] _v The new value for the w quaternion component. - public: inline void W(T _v) - { - this->qw = _v; - } - - /// \brief Stream insertion operator - /// \param[in] _out output stream - /// \param[in] _q quaternion to output - /// \return the stream - public: friend std::ostream &operator<<(std::ostream &_out, - const ignition::math::Quaternion &_q) - { - Vector3 v(_q.Euler()); - _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " " - << precision(v.Z(), 6); - return _out; - } - - /// \brief Stream extraction operator - /// \param[in] _in input stream - /// \param[in] _q Quaternion to read values into - /// \return The istream - public: friend std::istream &operator>>(std::istream &_in, - ignition::math::Quaternion &_q) - { - Angle roll, pitch, yaw; - - // Skip white spaces - _in.setf(std::ios_base::skipws); - _in >> roll >> pitch >> yaw; - - if (!_in.fail()) - { - _q.Euler(Vector3(*roll, *pitch, *yaw)); - } - - return _in; - } - - /// \brief w value of the quaternion - private: T qw; - - /// \brief x value of the quaternion - private: T qx; - - /// \brief y value of the quaternion - private: T qy; - - /// \brief z value of the quaternion - private: T qz; - }; - - template const Quaternion - Quaternion::Identity(1, 0, 0, 0); - - template const Quaternion - Quaternion::Zero(0, 0, 0, 0); - - typedef Quaternion Quaterniond; - typedef Quaternion Quaternionf; - typedef Quaternion Quaternioni; - } - } -} -#endif diff --git a/include/ignition/math/Rand.hh b/include/ignition/math/Rand.hh index c1f9a1340..9bbd2e77f 100644 --- a/include/ignition/math/Rand.hh +++ b/include/ignition/math/Rand.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,79 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_RAND_HH_ -#define IGNITION_MATH_RAND_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \def GeneratorType - /// \brief std::mt19937 - typedef std::mt19937 GeneratorType; - /// \def UniformRealDist - /// \brief std::uniform_real_distribution - typedef std::uniform_real_distribution UniformRealDist; - /// \def NormalRealDist - /// \brief std::normal_distribution - typedef std::normal_distribution NormalRealDist; - /// \def UniformIntDist - /// \brief std::uniform_int - typedef std::uniform_int_distribution UniformIntDist; - - /// \class Rand Rand.hh ignition/math/Rand.hh - /// \brief Random number generator class - class IGNITION_MATH_VISIBLE Rand - { - /// \brief Set the seed value. - /// \param[in] _seed The seed used to initialize the randon number - /// generator. - public: static void Seed(unsigned int _seed); - - /// \brief Get the seed value. - /// \return The seed value used to initialize the random number - /// generator. - public: static unsigned int Seed(); - - /// \brief Get a double from a uniform distribution - /// \param[in] _min Minimum bound for the random number - /// \param[in] _max Maximum bound for the random number - public: static double DblUniform(double _min = 0, double _max = 1); - - /// \brief Get a double from a normal distribution - /// \param[in] _mean Mean value for the distribution - /// \param[in] _sigma Sigma value for the distribution - public: static double DblNormal(double _mean = 0, double _sigma = 1); - - /// \brief Get an integer from a uniform distribution - /// \param[in] _min Minimum bound for the random number - /// \param[in] _max Maximum bound for the random number - public: static int32_t IntUniform(int _min, int _max); - - /// \brief Get an integer from a normal distribution - /// \param[in] _mean Mean value for the distribution - /// \param[in] _sigma Sigma value for the distribution - public: static int32_t IntNormal(int _mean, int _sigma); - - /// \brief Get a mutable reference to the seed (create the static - /// member if it hasn't been created yet). - private: static uint32_t &SeedMutable(); - - /// \brief Get a mutable reference to the random generator (create the - /// static member if it hasn't been created yet). - private: static GeneratorType &RandGenerator(); - }; - } - } -} -#endif diff --git a/include/ignition/math/Region3.hh b/include/ignition/math/Region3.hh index 339a9d91b..4b7564ef1 100644 --- a/include/ignition/math/Region3.hh +++ b/include/ignition/math/Region3.hh @@ -13,197 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_REGION3_HH_ -#define IGNITION_MATH_REGION3_HH_ + */ -#include -#include -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Region3 Region3.hh ignition/math/Region3.hh - /// \brief The Region3 class represents the cartesian product - /// of intervals Ix ✕ Iy ✕ Iz, one per axis, yielding an - /// axis-aligned region of R^3 space. It can be thought of as - /// an intersection of halfspaces. Regions may be open or - /// closed in their boundaries, if any. - /// - /// Note that the Region3 class is essentially a set R ⊆ R^3. - /// For 3D solid box semantics, use the `AxisAlignedBox` class - /// instead. - /// - /// ## Example - /// - /// \snippet examples/region3_example.cc complete - template - class Region3 - { - /// \brief An unbounded region (-∞, ∞) ✕ (-∞, ∞) ✕ (-∞, ∞) - public: static const Region3 &Unbounded; - - /// \brief Constructor - public: Region3() = default; - - /// \brief Constructor - /// \param[in] _ix x-axis interval - /// \param[in] _iy y-axis interval - /// \param[in] _iz z-axis interval - public: constexpr Region3( - Interval _ix, Interval _iy, Interval _iz) - : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz)) - { - } - - /// \brief Make an open region - /// \param[in] _xLeft leftmost x-axis interval value - /// \param[in] _xRight righmost x-axis interval value - /// \param[in] _yLeft leftmost y-axis interval value - /// \param[in] _yRight righmost y-axis interval value - /// \param[in] _zLeft leftmost z-axis interval value - /// \param[in] _zRight righmost z-axis interval value - /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`) - /// ✕ (`_zLeft`, `_zRight`) open region - public: static constexpr Region3 Open( - T _xLeft, T _yLeft, T _zLeft, - T _xRight, T _yRight, T _zRight) - { - return Region3(Interval::Open(_xLeft, _xRight), - Interval::Open(_yLeft, _yRight), - Interval::Open(_zLeft, _zRight)); - } - - /// \brief Make a closed region - /// \param[in] _xLeft leftmost x-axis interval value - /// \param[in] _xRight righmost x-axis interval value - /// \param[in] _yLeft leftmost y-axis interval value - /// \param[in] _yRight righmost y-axis interval value - /// \param[in] _zLeft leftmost z-axis interval value - /// \param[in] _zRight righmost z-axis interval value - /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`] - /// ✕ [`_zLeft`, `_zRight`] closed region - public: static constexpr Region3 Closed( - T _xLeft, T _yLeft, T _zLeft, - T _xRight, T _yRight, T _zRight) - { - return Region3(Interval::Closed(_xLeft, _xRight), - Interval::Closed(_yLeft, _yRight), - Interval::Closed(_zLeft, _zRight)); - } - - /// \brief Get the x-axis interval for the region - /// \return the x-axis interval - public: const Interval &Ix() const { return this->ix; } - - /// \brief Get the y-axis interval for the region - /// \return the y-axis interval - public: const Interval &Iy() const { return this->iy; } - - /// \brief Get the z-axis interval for the region - /// \return the z-axis interval - public: const Interval &Iz() const { return this->iz; } - - /// \brief Check if the region is empty - /// A region is empty if any of the intervals - /// it is defined with (i.e. Ix, Iy, Iz) are. - /// \return true if it is empty, false otherwise - public: bool Empty() const - { - return this->ix.Empty() || this->iy.Empty() || this->iz.Empty(); - } - - /// \brief Check if the region contains `_point` - /// \param[in] _point point to check for membership - /// \return true if it is contained, false otherwise - public: bool Contains(const Vector3 &_point) const - { - return (this->ix.Contains(_point.X()) && - this->iy.Contains(_point.Y()) && - this->iz.Contains(_point.Z())); - } - - /// \brief Check if the region contains `_other` region - /// \param[in] _other region to check for membership - /// \return true if it is contained, false otherwise - public: bool Contains(const Region3 &_other) const - { - return (this->ix.Contains(_other.ix) && - this->iy.Contains(_other.iy) && - this->iz.Contains(_other.iz)); - } - - /// \brief Check if the region intersects `_other` region - /// \param[in] _other region to check for intersection - /// \return true if it is contained, false otherwise - public: bool Intersects(const Region3& _other) const - { - return (this->ix.Intersects(_other.ix) && - this->iy.Intersects(_other.iy) && - this->iz.Intersects(_other.iz)); - } - - /// \brief Equality test operator - /// \param _other region to check for equality - /// \return true if regions are equal, false otherwise - public: bool operator==(const Region3 &_other) const - { - return this->Contains(_other) && _other.Contains(*this); - } - - /// \brief Inequality test operator - /// \param _other region to check for inequality - /// \return true if regions are unequal, false otherwise - public: bool operator!=(const Region3 &_other) const - { - return !this->Contains(_other) || !_other.Contains(*this); - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _r Region3 to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Region3 &_r) - { - return _out <<_r.ix << " x " << _r.iy << " x " << _r.iz; - } - - /// \brief The x-axis interval - private: Interval ix; - /// \brief The y-axis interval - private: Interval iy; - /// \brief The z-axis interval - private: Interval iz; - }; - - namespace detail { - template - constexpr Region3 gUnboundedRegion3( - Interval::Open(-std::numeric_limits::infinity(), - std::numeric_limits::infinity()), - Interval::Open(-std::numeric_limits::infinity(), - std::numeric_limits::infinity()), - Interval::Open(-std::numeric_limits::infinity(), - std::numeric_limits::infinity())); - } - template - const Region3 &Region3::Unbounded = detail::gUnboundedRegion3; - - using Region3f = Region3; - using Region3d = Region3; - } - } -} - -#endif diff --git a/include/ignition/math/RollingMean.hh b/include/ignition/math/RollingMean.hh index ff49dbdec..ce8f54411 100644 --- a/include/ignition/math/RollingMean.hh +++ b/include/ignition/math/RollingMean.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,78 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_ROLLINGMEAN_HH_ -#define IGNITION_MATH_ROLLINGMEAN_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declarations. - class RollingMeanPrivate; - - /// \brief A class that computes the mean over a series of data points. - /// The window size determines the maximum number of data points. The - /// oldest value is popped off when the window size is reached and - /// a new value is pushed in. - class IGNITION_MATH_VISIBLE RollingMean - { - /// \brief Constructor - /// \param[in] _windowSize The window size to use. This value will be - /// ignored if it is equal to zero. - public: explicit RollingMean(size_t _windowSize = 10); - - /// \brief Destructor. - public: ~RollingMean(); - - /// \brief Get the mean value. - /// \return The current mean value, or - /// std::numeric_limits::quiet_NaN() if data points are not - /// present. - public: double Mean() const; - - /// \brief Get the number of data points. - /// \return The number of datapoints. - public: size_t Count() const; - - /// \brief Insert a new value. - /// \param[in] _value New value to insert. - public: void Push(double _value); - - /// \brief Remove all the pushed values. - public: void Clear(); - - /// \brief Set the new window size. This will also clear the data. - /// Nothing happens if the _windowSize is zero. - /// \param[in] _windowSize The window size to use. - public: void SetWindowSize(size_t _windowSize); - - /// \brief Get the window size. - /// \return The window size. - public: size_t WindowSize() const; - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} - -#endif diff --git a/include/ignition/math/RotationSpline.hh b/include/ignition/math/RotationSpline.hh index 4c4c6b8d9..c403c7266 100644 --- a/include/ignition/math/RotationSpline.hh +++ b/include/ignition/math/RotationSpline.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,114 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_ROTATIONSPLINE_HH_ -#define IGNITION_MATH_ROTATIONSPLINE_HH_ + */ -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declare private data - class RotationSplinePrivate; - - /// \class RotationSpline RotationSpline.hh ignition/math/RotationSpline.hh - /// \brief Spline for rotations - class IGNITION_MATH_VISIBLE RotationSpline - { - /// \brief Constructor. Sets the autoCalc to true - public: RotationSpline(); - - /// \brief Destructor. Nothing is done - public: ~RotationSpline(); - - /// \brief Adds a control point to the end of the spline. - /// \param[in] _p control point - public: void AddPoint(const Quaterniond &_p); - - /// \brief Gets the detail of one of the control points of the spline. - /// \param[in] _index the index of the control point. _index is - /// clamped to [0, PointCount()-1]. - /// \remarks This point must already exist in the spline. - /// \return The quaternion at the specified point. - /// If there are no points, then a Quaterniond with a value of - /// [INF, INF, INF, INF] is returned. - public: const Quaterniond &Point(const unsigned int _index) const; - - /// \brief Gets the number of control points in the spline. - /// \return the count - public: unsigned int PointCount() const; - - /// \brief Clears all the points in the spline. - public: void Clear(); - - /// \brief Updates a single point in the spline. - /// \remarks This point must already exist in the spline. - /// \param[in] _index index - /// \param[in] _value the new control point value - /// \return True on success, false if _index is larger or equal than - /// PointCount(). - public: bool UpdatePoint(const unsigned int _index, - const Quaterniond &_value); - - /// \brief Returns an interpolated point based on a parametric - /// value over the whole series. - /// \remarks Given a t value between 0 and 1 representing the - /// parametric distance along the whole length of the spline, - /// this method returns an interpolated point. - /// \param[in] _t Parametric value. - /// \param[in] _useShortestPath Defines if rotation should take the - /// shortest possible path - /// \return The rotation, or [INF, INF, INF, INF] on error. Use - /// Quateriond::IsFinite() to check for an error - public: Quaterniond Interpolate(double _t, - const bool _useShortestPath = true); - - /// \brief Interpolates a single segment of the spline - /// given a parametric value. - /// \param[in] _fromIndex The point index to treat as t = 0. - /// _fromIndex + 1 is deemed to be t = 1 - /// \param[in] _t Parametric value - /// \param[in] _useShortestPath Defines if rotation should take the - /// shortest possible path - /// \return the rotation, or [INF, INF, INF, INF] on error. Use - /// Quateriond::IsFinite() to check for an error - public: Quaterniond Interpolate(const unsigned int _fromIndex, - const double _t, const bool _useShortestPath = true); - - /// \brief Tells the spline whether it should automatically calculate - /// tangents on demand as points are added. - /// \remarks The spline calculates tangents at each point automatically - /// based on the input points. Normally it does this every - /// time a point changes. However, if you have a lot of points - /// to add in one go, you probably don't want to incur this - /// overhead and would prefer to defer the calculation until - /// you are finished setting all the points. You can do this - /// by calling this method with a parameter of 'false'. Just - /// remember to manually call the recalcTangents method when - /// you are done. - /// \param[in] _autoCalc If true, tangents are calculated for you - /// whenever a point changes. If false, you must call reclacTangents to - /// recalculate them when it best suits. - public: void AutoCalculate(bool _autoCalc); - - /// \brief Recalculates the tangents associated with this spline. - /// \remarks If you tell the spline not to update on demand by calling - /// setAutoCalculate(false) then you must call this after - /// completing your updates to the spline points. - public: void RecalcTangents(); - - /// \brief Private data pointer - private: RotationSplinePrivate *dataPtr; - }; - } - } -} - -#endif diff --git a/include/ignition/math/SemanticVersion.hh b/include/ignition/math/SemanticVersion.hh index d3420e5a3..903aa1f30 100644 --- a/include/ignition/math/SemanticVersion.hh +++ b/include/ignition/math/SemanticVersion.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,152 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_MATH_SEMANTICVERSION_HH_ -#define IGNITION_MATH_SEMANTICVERSION_HH_ - -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declare private data class - class SemanticVersionPrivate; - - /// \class SemanticVersion SemanticVersion.hh - /// ignition/math/SemanticVersion.hh - /// \brief Version comparison class based on Semantic Versioning 2.0.0 - /// http://semver.org/ - /// Compares versions and converts versions from string. - class IGNITION_MATH_VISIBLE SemanticVersion - { - /// \brief Default constructor. Use the Parse function to populate - /// an instance with version information. - public: SemanticVersion(); - - /// \brief Constructor - /// \param[in] _v the string version. ex: "0.3.2" - public: explicit SemanticVersion(const std::string &_v); - - /// \brief Copy constructor - /// \param[in] _copy the other version - public: SemanticVersion(const SemanticVersion &_copy); - - /// \brief Assignment operator - /// \param[in] _other The version to assign from. - /// \return The reference to this instance - public: SemanticVersion &operator=(const SemanticVersion &_other); - - /// \brief Constructor - /// \param[in] _major The major number - /// \param[in] _minor The minor number - /// \param[in] _patch The patch number - /// \param[in] _prerelease The prerelease string - /// \param[in] _build The build metadata string - public: SemanticVersion(const unsigned int _major, - const unsigned int _minor = 0, - const unsigned int _patch = 0, - const std::string &_prerelease = "", - const std::string &_build = ""); - - /// \brief Destructor - public: ~SemanticVersion(); - - /// \brief Parse a version string and set the major, minor, patch - /// numbers, and prerelease and build strings. - /// \param[in] _versionStr The version string, such as "1.2.3-pr+123" - /// \return True on success. - public: bool Parse(const std::string &_versionStr); - - /// \brief Returns the version as a string - /// \return The semantic version string - public: std::string Version() const; - - /// \brief Get the major number - /// \return The major number - public: unsigned int Major() const; - - /// \brief Get the minor number - /// \return The minor number - public: unsigned int Minor() const; - - /// \brief Get the patch number - /// \return The patch number - public: unsigned int Patch() const; - - /// \brief Get the prerelease string. - /// \return Prelrease string, empty if a prerelease string was not - /// specified. - public: std::string Prerelease() const; - - /// \brief Get the build metadata string. Build meta data is not used - /// when determining precedence. - /// \return Build metadata string, empty if a build metadata string was - /// not specified. - public: std::string Build() const; - - /// \brief Less than comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is newer - public: bool operator<(const SemanticVersion &_other) const; - - /// \brief Less than or equal comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is newer or equal - public: bool operator<=(const SemanticVersion &_other) const; - - /// \brief Greater than comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is older - public: bool operator>(const SemanticVersion &_other) const; - - /// \brief Greater than or equal comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is older or the same - public: bool operator>=(const SemanticVersion &_other) const; - - /// \brief Equality comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is the same - public: bool operator==(const SemanticVersion &_other) const; - - /// \brief Inequality comparison operator - /// \param[in] _other The other version to compare to - /// \return True if _other version is different - public: bool operator!=(const SemanticVersion &_other) const; - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _v Semantic version to output - /// \return the stream - public: friend std::ostream &operator<<(std::ostream &_out, - const SemanticVersion &_v) - { - _out << _v.Version(); - return _out; - } - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Pointer to private data - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif diff --git a/include/ignition/math/SignalStats.hh b/include/ignition/math/SignalStats.hh index 2e1924ebe..0682f8d5e 100644 --- a/include/ignition/math/SignalStats.hh +++ b/include/ignition/math/SignalStats.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Open Source Robotics Foundation + * 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. @@ -13,243 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_SIGNALSTATS_HH_ -#define IGNITION_MATH_SIGNALSTATS_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \brief Forward declare private data class. - class SignalStatisticPrivate; - - /// \class SignalStatistic SignalStats.hh ignition/math/SignalStats.hh - /// \brief Statistical properties of a discrete time scalar signal. - class IGNITION_MATH_VISIBLE SignalStatistic - { - /// \brief Constructor - public: SignalStatistic(); - - /// \brief Destructor - public: virtual ~SignalStatistic(); - - /// \brief Copy constructor - /// \param[in] _ss SignalStatistic to copy - public: SignalStatistic(const SignalStatistic &_ss); - - /// \brief Get the current value of the statistical measure. - /// \return Current value of the statistical measure. - public: virtual double Value() const = 0; - - /// \brief Get a short version of the name of this statistical measure. - /// \return Short name of the statistical measure. - public: virtual std::string ShortName() const = 0; - - /// \brief Get number of data points in measurement. - /// \return Number of data points in measurement. - public: virtual size_t Count() const; - - /// \brief Add a new sample to the statistical measure. - /// \param[in] _data New signal data point. - public: virtual void InsertData(const double _data) = 0; - - /// \brief Forget all previous data. - public: virtual void Reset(); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Pointer to private data. - protected: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - - /// \class SignalMaximum SignalStats.hh ignition/math/SignalStats.hh - /// \brief Computing the maximum value of a discretely sampled signal. - class IGNITION_MATH_VISIBLE SignalMaximum : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "max" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \class SignalMean SignalStats.hh ignition/math/SignalStats.hh - /// \brief Computing the mean value of a discretely sampled signal. - class IGNITION_MATH_VISIBLE SignalMean : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "mean" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \class SignalMinimum SignalStats.hh ignition/math/SignalStats.hh - /// \brief Computing the minimum value of a discretely sampled signal. - class IGNITION_MATH_VISIBLE SignalMinimum : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "min" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \class SignalRootMeanSquare SignalStats.hh ignition/math/SignalStats.hh - /// \brief Computing the square root of the mean squared value - /// of a discretely sampled signal. - class IGNITION_MATH_VISIBLE SignalRootMeanSquare : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "rms" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \class SignalMaxAbsoluteValue SignalStats.hh - /// ignition/math/SignalStats.hh - /// \brief Computing the maximum of the absolute value - /// of a discretely sampled signal. - /// Also known as the maximum norm, infinity norm, or supremum norm. - class IGNITION_MATH_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "maxAbs" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \class SignalVariance SignalStats.hh ignition/math/SignalStats.hh - /// \brief Computing the incremental variance - /// of a discretely sampled signal. - class IGNITION_MATH_VISIBLE SignalVariance : public SignalStatistic - { - // Documentation inherited. - public: virtual double Value() const override; - - /// \brief Get a short version of the name of this statistical measure. - /// \return "var" - public: virtual std::string ShortName() const override; - - // Documentation inherited. - public: virtual void InsertData(const double _data) override; - }; - - /// \brief Forward declare private data class. - class SignalStatsPrivate; - - /// \class SignalStats SignalStats.hh ignition/math/SignalStats.hh - /// \brief Collection of statistics for a scalar signal. - class IGNITION_MATH_VISIBLE SignalStats - { - /// \brief Constructor - public: SignalStats(); - - /// \brief Destructor - public: ~SignalStats(); - - /// \brief Copy constructor - /// \param[in] _ss SignalStats to copy - public: SignalStats(const SignalStats &_ss); - - /// \brief Get number of data points in first statistic. - /// Technically you can have different numbers of data points - /// in each statistic if you call InsertStatistic after InsertData, - /// but this is not a recommended use case. - /// \return Number of data points in first statistic. - public: size_t Count() const; - - /// \brief Get the current values of each statistical measure, - /// stored in a map using the short name as the key. - /// \return Map with short name of each statistic as key - /// and value of statistic as the value. - public: std::map Map() const; - - /// \brief Add a new sample to the statistical measures. - /// \param[in] _data New signal data point. - public: void InsertData(const double _data); - - /// \brief Add a new type of statistic. - /// \param[in] _name Short name of new statistic. - /// Valid values include: - /// "maxAbs" - /// "mean" - /// "rms" - /// \return True if statistic was successfully added, - /// false if name was not recognized or had already - /// been inserted. - public: bool InsertStatistic(const std::string &_name); - - /// \brief Add multiple statistics. - /// \param[in] _names Comma-separated list of new statistics. - /// For example, all statistics could be added with: - /// "maxAbs,mean,rms" - /// \return True if all statistics were successfully added, - /// false if any names were not recognized or had already - /// been inserted. - public: bool InsertStatistics(const std::string &_names); - - /// \brief Forget all previous data. - public: void Reset(); - - /// \brief Assignment operator - /// \param[in] _s A SignalStats to copy - /// \return this - public: SignalStats &operator=(const SignalStats &_s); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif - diff --git a/include/ignition/math/SpeedLimiter.hh b/include/ignition/math/SpeedLimiter.hh index 0376ff7b4..76ff2c8d2 100644 --- a/include/ignition/math/SpeedLimiter.hh +++ b/include/ignition/math/SpeedLimiter.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,137 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_MATH_SYSTEMS_SPEEDLIMITER_HH_ -#define IGNITION_MATH_SYSTEMS_SPEEDLIMITER_HH_ - -#include -#include +#include #include -#include "ignition/math/Helpers.hh" - -namespace ignition -{ -namespace math -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // Forward declaration. - class SpeedLimiterPrivate; - - /// \brief Class to limit velocity, acceleration and jerk. - class IGNITION_MATH_VISIBLE SpeedLimiter - { - /// \brief Constructor. - /// There are no limits by default. - public: SpeedLimiter(); - - /// \brief Destructor. - public: ~SpeedLimiter(); - - /// \brief Set minimum velocity limit in m/s, usually <= 0. - /// \param[in] _lim Minimum velocity. - public: void SetMinVelocity(double _lim); - - /// \brief Get minimum velocity limit, defaults to negative infinity. - /// \return Minimum velocity. - public: double MinVelocity() const; - - /// \brief Set maximum velocity limit in m/s, usually >= 0. - /// \param[in] _lim Maximum velocity. - public: void SetMaxVelocity(double _lim); - - /// \brief Get maximum velocity limit, defaults to positive infinity. - /// \return Maximum velocity. - public: double MaxVelocity() const; - - /// \brief Set minimum acceleration limit in m/s^2, usually <= 0. - /// \param[in] _lim Minimum acceleration. - public: void SetMinAcceleration(double _lim); - - /// \brief Get minimum acceleration limit, defaults to negative infinity. - /// \return Minimum acceleration. - public: double MinAcceleration() const; - - /// \brief Set maximum acceleration limit in m/s^2, usually >= 0. - /// \param[in] _lim Maximum acceleration. - public: void SetMaxAcceleration(double _lim); - - /// \brief Get maximum acceleration limit, defaults to positive infinity. - /// \return Maximum acceleration. - public: double MaxAcceleration() const; - - /// \brief Set minimum jerk limit in m/s^3, usually <= 0. - /// \param[in] _lim Minimum jerk. - public: void SetMinJerk(double _lim); - - /// \brief Get minimum jerk limit, defaults to negative infinity. - /// \return Minimum jerk. - public: double MinJerk() const; - - /// \brief Set maximum jerk limit in m/s^3, usually >= 0. - /// \param[in] _lim Maximum jerk. - public: void SetMaxJerk(double _lim); - - /// \brief Get maximum jerk limit, defaults to positive infinity. - /// \return Maximum jerk. - public: double MaxJerk() const; - - /// \brief Limit velocity, acceleration and jerk. - /// \param [in, out] _vel Velocity to limit [m/s]. - /// \param [in] _prevVel Previous velocity to _vel [m/s]. - /// \param [in] _prevPrevVel Previous velocity to _prevVel [m/s]. - /// \param [in] _dt Time step. - /// \return Limiting difference, which is (out _vel - in _vel). - public: double Limit(double &_vel, - double _prevVel, - double _prevPrevVel, - std::chrono::steady_clock::duration _dt) const; - - /// \brief Limit the velocity. - /// \param [in, out] _vel Velocity to limit [m/s]. - /// \return Limiting difference, which is (out _vel - in _vel). - public: double LimitVelocity(double &_vel) const; - - /// \brief Limit the acceleration using a first-order backward difference - /// method. - /// \param [in, out] _vel Velocity [m/s]. - /// \param [in] _prevVel Previous velocity [m/s]. - /// \param [in] _dt Time step. - /// \return Limiting difference, which is (out _vel - in _vel). - public: double LimitAcceleration( - double &_vel, - double _prevVel, - std::chrono::steady_clock::duration _dt) const; - - /// \brief Limit the jerk using a second-order backward difference method. - /// \param [in, out] _vel Velocity to limit [m/s]. - /// \param [in] _prevVel Previous velocity to v [m/s]. - /// \param [in] _prevPrevVel Previous velocity to prevVel [m/s]. - /// \param [in] _dt Time step. - /// \return Limiting difference, which is (out _vel - in _vel). - /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. - public: double LimitJerk( - double &_vel, - double _prevVel, - double _prevPrevVel, - std::chrono::steady_clock::duration _dt) const; - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } -} -} - -#endif diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index 537bd109f..6f398538f 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,147 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_SPHERE_HH_ -#define IGNITION_MATH_SPHERE_HH_ + */ -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Plane.hh" - -namespace ignition -{ - namespace math - { - // Foward declarations - class SpherePrivate; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Sphere Sphere.hh ignition/math/Sphere.hh - /// \brief A representation of a sphere. - /// - /// The sphere class supports defining a sphere with a radius and - /// material properties. Radius is in meters. - /// See Material for more on material properties. - template - class Sphere - { - /// \brief Default constructor. The default radius is zero. - public: Sphere() = default; - - /// \brief Construct a sphere with a radius. - /// \param[in] _radius Radius of the sphere. - public: explicit Sphere(const Precision _radius); - - /// \brief Construct a sphere with a radius, material - /// \param[in] _radius Radius of the sphere. - /// \param[in] _mat Material property for the sphere. - public: Sphere(const Precision _radius, const Material &_mat); - - /// \brief Destructor - public: ~Sphere() = default; - - /// \brief Get the radius in meters. - /// \return The radius of the sphere in meters. - public: Precision Radius() const; - - /// \brief Set the radius in meters. - /// \param[in] _radius The radius of the sphere in meters. - public: void SetRadius(const Precision _radius); - - /// \brief Get the material associated with this sphere. - /// \return The material assigned to this sphere - public: const ignition::math::Material &Material() const; - - /// \brief Set the material associated with this sphere. - /// \param[in] _mat The material assigned to this sphere - public: void SetMaterial(const ignition::math::Material &_mat); - - /// \brief Get the mass matrix for this sphere. This function - /// is only meaningful if the sphere's radius and material have been set. - /// \param[out] _massMat The computed mass matrix will be stored - /// here. - /// \return False if computation of the mass matrix failed, which - /// could be due to an invalid radius (<=0) or density (<=0). - public: bool MassMatrix(MassMatrix3d &_massMat) const; - - /// \brief Check if this sphere is equal to the provided sphere. - /// Radius and material properties will be checked. - public: bool operator==(const Sphere &_sphere) const; - - /// \brief Check if this sphere is not equal to the provided sphere. - /// Radius and material properties will be checked. - public: bool operator!=(const Sphere &_sphere) const; - - /// \brief Get the volume of the sphere in m^3. - /// \return Volume of the sphere in m^3. - public: Precision Volume() const; - - /// \brief Get the volume of sphere below a given plane in m^3. - /// It is assumed that the center of the sphere is on the origin - /// \param[in] _plane The plane which slices this sphere, expressed - /// in the sphere's reference frame. - /// \return Volume below the sphere in m^3. - public: Precision VolumeBelow(const Plane &_plane) const; - - /// \brief Center of volume below the plane. This is useful for example - /// when calculating where buoyancy should be applied. - /// \param[in] _plane The plane which slices this sphere, expressed - /// in the sphere's reference frame. - /// \return The center of volume if there is anything under the plane, - /// otherwise return a std::nullopt. Expressed in the sphere's reference - /// frame. - public: std::optional> - CenterOfVolumeBelow(const Plane &_plane) const; - - /// \brief Compute the sphere's density given a mass value. The - /// sphere is assumed to be solid with uniform density. This - /// function requires the sphere's radius to be set to a - /// value greater than zero. The Material of the sphere is ignored. - /// \param[in] _mass Mass of the sphere, in kg. This value should be - /// greater than zero. - /// \return Density of the sphere in kg/m^3. A negative value is - /// returned if radius or _mass is <= 0. - public: Precision DensityFromMass(const Precision _mass) const; - - /// \brief Set the density of this sphere based on a mass value. - /// Density is computed using - /// Precision DensityFromMass(const Precision _mass) const. The - /// sphere is assumed to be solid with uniform density. This - /// function requires the sphere's radius to be set to a - /// value greater than zero. The existing Material density value is - /// overwritten only if the return value from this true. - /// \param[in] _mass Mass of the sphere, in kg. This value should be - /// greater than zero. - /// \return True if the density was set. False is returned if the - /// sphere's radius or the _mass value are <= 0. - /// \sa Precision DensityFromMass(const Precision _mass) const - public: bool SetDensityFromMass(const Precision _mass); - - /// \brief Radius of the sphere. - private: Precision radius = 0.0; - - /// \brief the sphere's material. - private: ignition::math::Material material; - }; - - /// \typedef Sphere Spherei - /// \brief Sphere with integer precision. - typedef Sphere Spherei; - - /// \typedef Sphere Sphered - /// \brief Sphere with double precision. - typedef Sphere Sphered; - - /// \typedef Sphere Spheref - /// \brief Sphere with float precision. - typedef Sphere Spheref; - } - } -} -#include "ignition/math/detail/Sphere.hh" - -#endif +#include +#include diff --git a/include/ignition/math/SphericalCoordinates.hh b/include/ignition/math/SphericalCoordinates.hh index 120407ab6..c4109bce2 100644 --- a/include/ignition/math/SphericalCoordinates.hh +++ b/include/ignition/math/SphericalCoordinates.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,254 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_SPHERICALCOORDINATES_HH_ -#define IGNITION_MATH_SPHERICALCOORDINATES_HH_ + */ -#include -#include - -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - class SphericalCoordinatesPrivate; - - /// \brief Convert spherical coordinates for planetary surfaces. - class IGNITION_MATH_VISIBLE SphericalCoordinates - { - /// \enum SurfaceType - /// \brief Unique identifiers for planetary surface models. - public: enum SurfaceType - { - /// \brief Model of reference ellipsoid for earth, based on - /// WGS 84 standard. see wikipedia: World_Geodetic_System - EARTH_WGS84 = 1 - }; - - /// \enum CoordinateType - /// \brief Unique identifiers for coordinate types. - public: enum CoordinateType - { - /// \brief Latitude, Longitude and Altitude by SurfaceType - SPHERICAL = 1, - - /// \brief Earth centered, earth fixed Cartesian - ECEF = 2, - - /// \brief Local tangent plane (East, North, Up) - GLOBAL = 3, - - /// \brief Heading-adjusted tangent plane (X, Y, Z) - /// This has kept a bug for backwards compatibility, use - /// LOCAL2 for the correct behaviour. - LOCAL = 4, - - /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL2 = 5 - }; - - /// \brief Constructor. - public: SphericalCoordinates(); - - /// \brief Constructor with surface type input. - /// \param[in] _type SurfaceType specification. - public: explicit SphericalCoordinates(const SurfaceType _type); - - /// \brief Constructor with surface type, angle, and elevation inputs. - /// \param[in] _type SurfaceType specification. - /// \param[in] _latitude Reference latitude. - /// \param[in] _longitude Reference longitude. - /// \param[in] _elevation Reference elevation. - /// \param[in] _heading Heading offset. - public: SphericalCoordinates(const SurfaceType _type, - const ignition::math::Angle &_latitude, - const ignition::math::Angle &_longitude, - const double _elevation, - const ignition::math::Angle &_heading); - - /// \brief Copy constructor. - /// \param[in] _sc Spherical coordinates to copy. - public: SphericalCoordinates(const SphericalCoordinates &_sc); - - /// \brief Destructor. - public: ~SphericalCoordinates(); - - /// \brief Convert a Cartesian position vector to geodetic coordinates. - /// This performs a `PositionTransform` from LOCAL to SPHERICAL. - /// - /// There's a known bug with this computation that can't be fixed on - /// version 6 to avoid behaviour changes. Directly call - /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results. - /// Note that `PositionTransform` returns spherical coordinates in - /// radians. - /// - /// \param[in] _xyz Cartesian position vector in the heading-adjusted - /// world frame. - /// \return Cooordinates: geodetic latitude (deg), longitude (deg), - /// altitude above sea level (m). - public: ignition::math::Vector3d SphericalFromLocalPosition( - const ignition::math::Vector3d &_xyz) const; - - /// \brief Convert a Cartesian velocity vector in the local frame - /// to a global Cartesian frame with components East, North, Up. - /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` - /// - /// There's a known bug with this computation that can't be fixed on - /// version 6 to avoid behaviour changes. Directly call - /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results. - /// - /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted - /// world frame. - /// \return Rotated vector with components (x,y,z): (East, North, Up). - public: ignition::math::Vector3d GlobalFromLocalVelocity( - const ignition::math::Vector3d &_xyz) const; - - /// \brief Convert a string to a SurfaceType. - /// Allowed values: ["EARTH_WGS84"]. - /// \param[in] _str String to convert. - /// \return Conversion to SurfaceType. - public: static SurfaceType Convert(const std::string &_str); - - /// \brief Convert a SurfaceType to a string. - /// \param[in] _type Surface type - /// \return Type as string - public: static std::string Convert(SurfaceType _type); - - /// \brief Get the distance between two points expressed in geographic - /// latitude and longitude. It assumes that both points are at sea level. - /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents - /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W. - /// \param[in] _latA Latitude of point A. - /// \param[in] _lonA Longitude of point A. - /// \param[in] _latB Latitude of point B. - /// \param[in] _lonB Longitude of point B. - /// \return Distance in meters. - public: static double Distance(const ignition::math::Angle &_latA, - const ignition::math::Angle &_lonA, - const ignition::math::Angle &_latB, - const ignition::math::Angle &_lonB); - - /// \brief Get SurfaceType currently in use. - /// \return Current SurfaceType value. - public: SurfaceType Surface() const; - - /// \brief Get reference geodetic latitude. - /// \return Reference geodetic latitude. - public: ignition::math::Angle LatitudeReference() const; - - /// \brief Get reference longitude. - /// \return Reference longitude. - public: ignition::math::Angle LongitudeReference() const; - - /// \brief Get reference elevation in meters. - /// \return Reference elevation. - public: double ElevationReference() const; - - /// \brief Get heading offset for the reference frame, expressed as - /// angle from East to x-axis, or equivalently - /// from North to y-axis. - /// \return Heading offset of reference frame. - public: ignition::math::Angle HeadingOffset() const; - - /// \brief Set SurfaceType for planetary surface model. - /// \param[in] _type SurfaceType value. - public: void SetSurface(const SurfaceType &_type); - - /// \brief Set reference geodetic latitude. - /// \param[in] _angle Reference geodetic latitude. - public: void SetLatitudeReference(const ignition::math::Angle &_angle); - - /// \brief Set reference longitude. - /// \param[in] _angle Reference longitude. - public: void SetLongitudeReference(const ignition::math::Angle &_angle); - - /// \brief Set reference elevation above sea level in meters. - /// \param[in] _elevation Reference elevation. - public: void SetElevationReference(const double _elevation); - - /// \brief Set heading angle offset for the frame. - /// \param[in] _angle Heading offset for the frame. - public: void SetHeadingOffset(const ignition::math::Angle &_angle); - - /// \brief Convert a geodetic position vector to Cartesian coordinates. - /// This performs a `PositionTransform` from SPHERICAL to LOCAL. - /// \param[in] _latLonEle Geodetic position in the planetary frame of - /// reference. X: latitude (deg), Y: longitude (deg), X: altitude. - /// \return Cartesian position vector in the heading-adjusted world frame. - public: ignition::math::Vector3d LocalFromSphericalPosition( - const ignition::math::Vector3d &_latLonEle) const; - - /// \brief Convert a Cartesian velocity vector with components East, - /// North, Up to a local cartesian frame vector XYZ. - /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` - /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up). - /// \return Cartesian vector in the world frame. - public: ignition::math::Vector3d LocalFromGlobalVelocity( - const ignition::math::Vector3d &_xyz) const; - - /// \brief Update coordinate transformation matrix with reference location - public: void UpdateTransformationMatrix(); - - /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame - /// Spherical coordinates use radians, while the other frames use meters. - /// \param[in] _pos Position vector in frame defined by parameter _in - /// \param[in] _in CoordinateType for input - /// \param[in] _out CoordinateType for output - /// \return Transformed coordinate using cached origin. - public: ignition::math::Vector3d - PositionTransform(const ignition::math::Vector3d &_pos, - const CoordinateType &_in, const CoordinateType &_out) const; - - /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame - /// Spherical coordinates use radians, while the other frames use meters. - /// \param[in] _vel Velocity vector in frame defined by parameter _in - /// \param[in] _in CoordinateType for input - /// \param[in] _out CoordinateType for output - /// \return Transformed velocity vector - public: ignition::math::Vector3d VelocityTransform( - const ignition::math::Vector3d &_vel, - const CoordinateType &_in, const CoordinateType &_out) const; - - /// \brief Equality operator, result = this == _sc - /// \param[in] _sc Spherical coordinates to check for equality - /// \return true if this == _sc - public: bool operator==(const SphericalCoordinates &_sc) const; - - /// \brief Inequality - /// \param[in] _sc Spherical coordinates to check for inequality - /// \return true if this != _sc - public: bool operator!=(const SphericalCoordinates &_sc) const; - - /// \brief Assignment operator - /// \param[in] _sc The spherical coordinates to copy from. - /// \return this - public: SphericalCoordinates &operator=( - const SphericalCoordinates &_sc); - - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \internal - /// \brief Pointer to the private data - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif diff --git a/include/ignition/math/Spline.hh b/include/ignition/math/Spline.hh index ee2658223..47688d66e 100644 --- a/include/ignition/math/Spline.hh +++ b/include/ignition/math/Spline.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,254 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -// Note: Originally cribbed from Ogre3d. Modified to implement Cardinal -// spline and catmull-rom spline -#ifndef IGNITION_MATH_SPLINE_HH_ -#define IGNITION_MATH_SPLINE_HH_ + */ -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declare private classes - class ControlPoint; - class SplinePrivate; - - /// \class Spline Spline.hh ignition/math/Spline.hh - /// \brief Splines - class IGNITION_MATH_VISIBLE Spline - { - /// \brief constructor - public: Spline(); - - /// \brief destructor - public: ~Spline(); - - /// \brief Sets the tension parameter. - /// \remarks A value of 0 results in a Catmull-Rom - /// spline. - /// \param[in] _t Tension value between 0.0 and 1.0 - public: void Tension(double _t); - - /// \brief Gets the tension value. - /// \return the value of the tension, which is between 0.0 and 1.0. - public: double Tension() const; - - /// \brief Gets spline arc length. - /// \return arc length or INF on error. - public: double ArcLength() const; - - /// \brief Sets spline arc length up to - /// a given parameter value \p _t. - /// \param[in] _t parameter value (range 0 to 1). - /// \return arc length up to \p _t or INF on error. - public: double ArcLength(const double _t) const; - - /// \brief Sets a spline segment arc length. - /// \param[in] _index of the spline segment. - /// \param[in] _t parameter value (range 0 to 1). - /// \return arc length of a given segment up to - /// \p _t or INF on error. - public: double ArcLength(const unsigned int _index, - const double _t) const; - - /// \brief Adds a single control point to the - /// end of the spline. - /// \param[in] _p control point value to add. - public: void AddPoint(const Vector3d &_p); - - /// \brief Adds a single control point to the end - /// of the spline with fixed tangent. - /// \param[in] _p control point value to add. - /// \param[in] _t tangent at \p _p. - public: void AddPoint(const Vector3d &_p, const Vector3d &_t); - - /// \brief Adds a single control point to the end - /// of the spline. - /// \param[in] _cp control point to add. - /// \param[in] _fixed whether this control point - /// should not be subject to tangent recomputation. - private: void AddPoint(const ControlPoint &_cp, const bool _fixed); - - /// \brief Gets the value for one of the control points - /// of the spline. - /// \param[in] _index the control point index. - /// \return the control point value, or [INF, INF, INF] - /// on error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d Point(const unsigned int _index) const; - - /// \brief Gets the tangent value for one of the control points - /// of the spline. - /// \param[in] _index the control point index. - /// \return the control point tangent, or [INF, INF, INF] - /// on error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d Tangent(const unsigned int _index) const; - - /// \brief Gets the mth derivative for one of the control points - /// of the spline. - /// \param[in] _index the control point index. - /// \param[in] _mth derivative order. - /// \return the control point mth derivative, or [INF, INF, INF] - /// on error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d MthDerivative(const unsigned int _index, - const unsigned int _mth) const; - - /// \brief Gets the number of control points in the spline. - /// \return the count - public: size_t PointCount() const; - - /// \brief Clears all the points in the spline. - public: void Clear(); - - /// \brief Updates a single control point value in the spline, - /// keeping its tangent. - /// \param[in] _index the control point index. - /// \param[in] _p the new control point value. - /// \return True on success. - public: bool UpdatePoint(const unsigned int _index, - const Vector3d &_p); - - /// \brief Updates a single control point in the spline, along - /// with its tangent. - /// \param[in] _index the control point index. - /// \param[in] _p the new control point value. - /// \param[in] _t the new control point tangent. - /// \return True on success. - public: bool UpdatePoint(const unsigned int _index, - const Vector3d &_p, - const Vector3d &_t); - - /// \brief Updates a single control point in the spline. - /// \param[in] _index the control point index - /// \param[in] _cp the new control point - /// \param[in] _fixed whether the new control point should not be - /// subject to tangent recomputation - /// \return True on success. - private: bool UpdatePoint(const unsigned int _index, - const ControlPoint &_cp, - const bool _fixed); - - /// \brief Interpolates a point on the spline - /// at parameter value \p _t. - /// \remarks Parameter value is normalized over the - /// whole spline arc length. Arc length is assumed - /// to be linear with the parameter. - /// \param[in] _t parameter value (range 0 to 1). - /// \return the interpolated point, or - /// [INF, INF, INF] on error. Use - /// Vector3d::IsFinite() to check for an error. - public: Vector3d Interpolate(const double _t) const; - - /// \brief Interpolates a point on a segment of the spline - /// at parameter value \p _t. - /// \remarks Parameter value is normalized over the - /// segment arc length. Arc length is assumed - /// to be linear with the parameter. - /// \param[in] _fromIndex The point index to treat as t = 0. - /// fromIndex + 1 is deemed to be t = 1. - /// \param[in] _t parameter value (range 0 to 1). - /// \return the interpolated point, or [INF, INF, INF] on - /// error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d Interpolate(const unsigned int _fromIndex, - const double _t) const; - - /// \brief Interpolates a tangent on the spline at - /// parameter value \p _t. - /// \remarks Parameter value is normalized over the - /// whole spline arc length. Arc length is assumed - /// to be linear with the parameter. - /// \param[in] _t parameter value (range 0 to 1). - /// \return the interpolated point, or [INF, INF, INF] - /// on error. Use Vector3d::IsFinte() to check for an error. - public: Vector3d InterpolateTangent(const double _t) const; - - /// \brief Interpolates the tangent on a segment of the spline - /// at parameter value \p _t. - /// \remarks Parameter value is normalized over the - /// segment arc length. Arc length is assumed - /// to be linear with the parameter. - /// \param[in] _fromIndex the point index to treat as t = 0. - /// fromIndex + 1 is deemed to be t = 1. - /// \param[in] _t parameter value (range 0 to 1). - /// \return the interpolated point, or [INF, INF, INF] on - /// error. Use Vector3d::IsFinte() to check for an error. - public: Vector3d InterpolateTangent(const unsigned int _fromIndex, - const double _t) const; - - /// \brief Interpolates the mth derivative of the spline at - /// parameter value \p _t. - /// \param[in] _mth order of curve derivative to interpolate. - /// \param[in] _1 parameter value (range 0 to 1). - /// \return the interpolated mth derivative, or [INF, INF, INF] - /// on error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d InterpolateMthDerivative(const unsigned int _mth, - const double _1) const; - - /// \brief Interpolates the mth derivative of a segment of the spline - /// at parameter value \p _t. - /// \remarks Parameter value is normalized over the segment - /// arc length. Arc length is assumed to be linear with the parameter. - /// \param[in] _fromIndex point index to treat as t = 0, fromIndex + 1 - /// is deemed to be t = 1. - /// \param[in] _mth order of curve derivative to interpolate. - /// \param[in] _s parameter value (range 0 to 1). - /// \return the interpolated mth derivative, or [INF, INF, INF] on - /// error. Use Vector3d::IsFinite() to check for an error. - public: Vector3d InterpolateMthDerivative(const unsigned int _fromIndex, - const unsigned int _mth, - const double _s) const; - - /// \brief Tells the spline whether it should automatically - /// calculate tangents on demand as points are added. - /// \remarks The spline calculates tangents at each point - /// automatically based on the input points. Normally it - /// does this every time a point changes. However, if you - /// have a lot of points to add in one go, you probably - /// don't want to incur this overhead and would prefer to - /// defer the calculation until you are finished setting all - /// the points. You can do this by calling this method with a - /// parameter of 'false'. Just remember to manually call the - /// recalcTangents method when you are done. - /// \param[in] _autoCalc If true, tangents are calculated for you whenever - /// a point changes. If false, you must call RecalcTangents to - /// recalculate them when it best suits. - public: void AutoCalculate(bool _autoCalc); - - /// \brief Recalculates the tangents associated with this spline. - /// \remarks If you tell the spline not to update on demand by - /// calling setAutoCalculate(false) then you must call this - /// after completing your updates to the spline points. - public: void RecalcTangents(); - - /// \brief Rebuilds spline segments. - private: void Rebuild(); - - /// \internal - /// \brief Maps \p _t parameter value over the whole spline - /// to the right segment (starting at point \p _index) with - /// the proper parameter value fraction \p _fraction. - /// \remarks Arc length is assumed to be linear with the parameter. - /// \param[in] _t parameter value over the whole spline (range 0 to 1). - /// \param[out] _index point index at which the segment starts. - /// \param[out] _fraction parameter value fraction for the given segment. - /// \return True on success. - private: bool MapToSegment(const double _t, - unsigned int &_index, - double &_fraction) const; - - /// \internal - /// \brief Private data pointer - private: SplinePrivate *dataPtr; - }; - } - } -} -#endif diff --git a/include/ignition/math/Stopwatch.hh b/include/ignition/math/Stopwatch.hh index f469672f5..4354dda68 100644 --- a/include/ignition/math/Stopwatch.hh +++ b/include/ignition/math/Stopwatch.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,145 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_MATH_STOPWATCH_HH_ -#define IGNITION_MATH_STOPWATCH_HH_ - -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Use a steady clock - using clock = std::chrono::steady_clock; - - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declarations. - class StopwatchPrivate; - - // - /// \class Stopwatch Stopwatch.hh ignition/math/Stopwatch.hh - /// \brief The Stopwatch keeps track of time spent in the run state, - /// accessed through ElapsedRunTime(), and time spent in the stop state, - /// accessed through ElapsedStopTime(). Elapsed run time starts accumulating - /// after the first call to Start(). Elapsed stop time starts - /// accumulation after Start() has been called followed by Stop(). The - /// stopwatch can be reset with the Reset() function. - /// - /// # Example usage - /// - /// ```{.cpp} - /// ignition::math::Stopwatch watch; - /// watch.Start(); - /// - /// // do something... - /// - /// std::cout << "Elapsed time is " - /// << std::chrono::duration_cast( - /// timeSys.ElapsedRunTime()).count() << " ms\n"; - /// watch.Stop(); - /// ``` - class IGNITION_MATH_VISIBLE Stopwatch - { - /// \brief Constructor. - public: Stopwatch(); - - /// \brief Copy constructor - /// \param[in] _watch The stop watch to copy. - public: Stopwatch(const Stopwatch &_watch); - - /// \brief Move constructor - /// \param[in] _watch The stop watch to move. - public: Stopwatch(Stopwatch &&_watch) noexcept; - - /// \brief Destructor. - public: virtual ~Stopwatch(); - - /// \brief Start the stopwatch. - /// \param[in] _reset If true the stopwatch is reset first. - /// \return True if the the stopwatch was started. This will return - /// false if the stopwatch was already running. - public: bool Start(const bool _reset = false); - - /// \brief Get the time when the stopwatch was started. - /// \return The time when stopwatch was started, or - /// std::chrono::steady_clock::time_point::min() if the stopwatch - /// has not been started. - public: clock::time_point StartTime() const; - - /// \brief Stop the stopwatch - /// \return True if the stopwatch was stopped. This will return false - /// if the stopwatch is not running. - public: bool Stop(); - - /// \brief Get the time when the stopwatch was last stopped. - /// \return The time when stopwatch was last stopped, or - /// std::chrono::steady_clock::time_point::min() if the stopwatch - /// has never been stopped. - public: clock::time_point StopTime() const; - - /// \brief Get whether the stopwatch is running. - /// \return True if the stopwatch is running. - public: bool Running() const; - - /// \brief Reset the stopwatch. This resets the start time, stop time, - /// elapsed duration and elapsed stop duration. - public: void Reset(); - - /// \brief Get the amount of time that the stop watch has been - /// running. This is the total amount of run time, spannning all start - /// and stop calls. The Reset function or passing true to the Start - /// function will reset this value. - /// \return Total amount of elapsed run time. - public: clock::duration ElapsedRunTime() const; - - /// \brief Get the amount of time that the stop watch has been - /// stopped. This is the total amount of stop time, spannning all start - /// and stop calls. The Reset function or passing true to the Start - /// function will reset this value. - /// \return Total amount of elapsed stop time. - public: clock::duration ElapsedStopTime() const; - - /// \brief Equality operator. - /// \param[in] _watch The watch to compare. - /// \return True if this watch equals the provided watch. - public: bool operator==(const Stopwatch &_watch) const; - - /// \brief Inequality operator. - /// \param[in] _watch The watch to compare. - /// \return True if this watch does not equal the provided watch. - public: bool operator!=(const Stopwatch &_watch) const; - - /// \brief Copy assignment operator - /// \param[in] _watch The stop watch to copy. - /// \return Reference to this. - public: Stopwatch &operator=(const Stopwatch &_watch); - - /// \brief Move assignment operator - /// \param[in] _watch The stop watch to move. - /// \return Reference to this. - public: Stopwatch &operator=(Stopwatch &&_watch); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif diff --git a/include/ignition/math/Temperature.hh b/include/ignition/math/Temperature.hh index 067137ec8..6fc6ea4a4 100644 --- a/include/ignition/math/Temperature.hh +++ b/include/ignition/math/Temperature.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,411 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_TEMPERATURE_HH_ -#define IGNITION_MATH_TEMPERATURE_HH_ - -#include -#include + */ +#include #include -#include "ignition/math/Helpers.hh" - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declare private data class. - class TemperaturePrivate; - - /// \brief A class that stores temperature information, and allows - /// conversion between different units. - /// - /// This class is mostly for convenience. It can be used to easily - /// convert between temperature units and encapsulate temperature values. - /// - /// The default unit is Kelvin. Most functions that accept a double - /// value will assume the double is Kelvin. The exceptions are a few of - /// the conversion functions, such as CelsiusToFahrenheit. Similarly, - /// most doubles that are returned will be in Kelvin. - /// - /// ## Example usage ## - /// - /// ### Convert from Kelvin to Celsius ### - /// - /// double celsius = ignition::math::Temperature::KelvinToCelsius(2.5); - /// - /// ### Create and use a Temperature object ### - /// - /// ignition::math::Temperature temp(123.5); - /// std::cout << "Temperature in Kelvin = " << temp << std::endl; - /// std::cout << "Temperature in Celsius = " - /// << temp.Celsius() << std::endl; - /// - /// temp += 100.0; - /// std::cout << "Temperature + 100.0 = " << temp << "K" << std::endl; - /// - /// ignition::math::Temperature newTemp(temp); - /// newTemp += temp + 23.5; - /// std::cout << "Copied the temp object and added 23.5K. newTemp = " - /// << newTemp.Fahrenheit() << "F" << std::endl; - /// - class IGNITION_MATH_VISIBLE Temperature - { - /// \brief Default constructor - public: Temperature(); - - /// \brief Kelvin value constructor. This is a conversion constructor - /// \param[in] _temp Temperature in Kelvin - // cppcheck-suppress noExplicitConstructor - public: Temperature(const double _temp); - - /// \brief Copy constructor - /// \param[in] _temp Temperature object to copy. - public: Temperature(const Temperature &_temp); - - /// \brief Destructor - public: virtual ~Temperature(); - - /// \brief Convert Kelvin to Celsius - /// \param[in] _temp Temperature in Kelvin - /// \return Temperature in Celsius - public: static double KelvinToCelsius(const double _temp); - - /// \brief Convert Kelvin to Fahrenheit - /// \param[in] _temp Temperature in Kelvin - /// \return Temperature in Fahrenheit - public: static double KelvinToFahrenheit(const double _temp); - - /// \brief Convert Celsius to Fahrenheit - /// \param[in] _temp Temperature in Celsius - /// \return Temperature in Fahrenheit - public: static double CelsiusToFahrenheit(const double _temp); - - /// \brief Convert Celsius to Kelvin - /// \param[in] _temp Temperature in Celsius - /// \return Temperature in Kelvin - public: static double CelsiusToKelvin(const double _temp); - - /// \brief Convert Fahrenheit to Celsius - /// \param[in] _temp Temperature in Fahrenheit - /// \return Temperature in Celsius - public: static double FahrenheitToCelsius(const double _temp); - - /// \brief Convert Fahrenheit to Kelvin - /// \param[in] _temp Temperature in Fahrenheit - /// \return Temperature in Kelvin - public: static double FahrenheitToKelvin(const double _temp); - - /// \brief Set the temperature from a Kelvin value - /// \param[in] _temp Temperature in Kelvin - public: void SetKelvin(const double _temp); - - /// \brief Set the temperature from a Celsius value - /// \param[in] _temp Temperature in Celsius - public: void SetCelsius(const double _temp); - - /// \brief Set the temperature from a Fahrenheit value - /// \param[in] _temp Temperature in Fahrenheit - public: void SetFahrenheit(const double _temp); - - /// \brief Get the temperature in Kelvin - /// \return Temperature in Kelvin - public: double Kelvin() const; - - /// \brief Get the temperature in Celsius - /// \return Temperature in Celsius - public: double Celsius() const; - - /// \brief Get the temperature in Fahrenheit - /// \return Temperature in Fahrenheit - public: double Fahrenheit() const; - - /// \brief Accessor operator - /// \return Temperature in Kelvin - /// \sa Kelvin() - public: double operator()() const; - - /// \brief Assignment operator - /// \param[in] _temp Temperature in Kelvin - /// \return Reference to this instance - public: Temperature &operator=(const double _temp); - - /// \brief Assignment operator - /// \param[in] _temp Temperature object - /// \return Reference to this instance - public: Temperature &operator=(const Temperature &_temp); - - /// \brief Addition operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator+(const double _temp) const; - - /// \brief Addition operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator+(const Temperature &_temp) const; - - /// \brief Addition operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator+(const double _temp); - - /// \brief Addition operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator+(const Temperature &_temp); - - /// \brief Addition operator for double type. - /// \param[in] _t Temperature in kelvin - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: friend Temperature operator+(double _t, const Temperature &_temp) - { - return _t + _temp.Kelvin(); - } - - /// \brief Addition assignment operator - /// \param[in] _temp Temperature in Kelvin - /// \return Reference to this instance - public: const Temperature &operator+=(const double _temp); - - /// \brief Addition assignment operator - /// \param[in] _temp Temperature object - /// \return Reference to this instance - public: const Temperature &operator+=(const Temperature &_temp); - - /// \brief Subtraction operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator-(const double _temp); - - /// \brief Subtraction operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator-(const Temperature &_temp); - - /// \brief Subtraction operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator-(const double _temp) const; - - /// \brief Subtraction operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator-(const Temperature &_temp) const; - - /// \brief Subtraction operator for double type. - /// \param[in] _t Temperature in kelvin - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: friend Temperature operator-(double _t, const Temperature &_temp) - { - return _t - _temp.Kelvin(); - } - - /// \brief Subtraction assignment operator - /// \param[in] _temp Temperature in Kelvin - /// \return Reference to this instance - public: const Temperature &operator-=(const double _temp); - - /// \brief Subtraction assignment operator - /// \param[in] _temp Temperature object - /// \return Reference to this instance - public: const Temperature &operator-=(const Temperature &_temp); - - /// \brief Multiplication operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator*(const double _temp); - - /// \brief Multiplication operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator*(const Temperature &_temp); - - /// \brief Multiplication operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator*(const double _temp) const; - - /// \brief Multiplication operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator*(const Temperature &_temp) const; - - /// \brief Multiplication operator for double type. - /// \param[in] _t Temperature in kelvin - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: friend Temperature operator*(double _t, const Temperature &_temp) - { - return _t * _temp.Kelvin(); - } - - /// \brief Multiplication assignment operator - /// \param[in] _temp Temperature in Kelvin - /// \return Reference to this instance - public: const Temperature &operator*=(const double _temp); - - /// \brief Multiplication assignment operator - /// \param[in] _temp Temperature object - /// \return Reference to this instance - public: const Temperature &operator*=(const Temperature &_temp); - - /// \brief Division operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator/(const double _temp); - - /// \brief Division operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator/(const Temperature &_temp); - - /// \brief Division operator - /// \param[in] _temp Temperature in Kelvin - /// \return Resulting temperature - public: Temperature operator/(const double _temp) const; - - /// \brief Division operator - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: Temperature operator/(const Temperature &_temp) const; - - /// \brief Division operator for double type. - /// \param[in] _t Temperature in kelvin - /// \param[in] _temp Temperature object - /// \return Resulting temperature - public: friend Temperature operator/(double _t, const Temperature &_temp) - { - return _t / _temp.Kelvin(); - } - - /// \brief Division assignment operator - /// \param[in] _temp Temperature in Kelvin - /// \return Reference to this instance - public: const Temperature &operator/=(const double _temp); - - /// \brief Division assignment operator - /// \param[in] _temp Temperature object - /// \return Reference to this instance - public: const Temperature &operator/=(const Temperature &_temp); - - /// \brief Equal to operator - /// \param[in] _temp The temperature to compare - /// \return true if the temperatures are the same, false otherwise - public: bool operator==(const Temperature &_temp) const; - - /// \brief Equal to operator, where the value of _temp is assumed to - /// be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return true if the temperatures are the same, false otherwise - public: bool operator==(const double _temp) const; - - /// \brief Inequality to operator - /// \param[in] _temp The temperature to compare - /// \return false if the temperatures are the same, true otherwise - public: bool operator!=(const Temperature &_temp) const; - - /// \brief Inequality to operator, where the value of _temp is assumed to - /// be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return false if the temperatures are the same, true otherwise - public: bool operator!=(const double _temp) const; - - /// \brief Less than to operator - /// \param[in] _temp The temperature to compare - /// \return True if this is less than _temp. - public: bool operator<(const Temperature &_temp) const; - - /// \brief Less than operator, where the value of _temp is assumed to - /// be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return True if this is less than _temp. - public: bool operator<(const double _temp) const; - - /// \brief Less than or equal to operator - /// \param[in] _temp The temperature to compare - /// \return True if this is less than or equal _temp. - public: bool operator<=(const Temperature &_temp) const; - - /// \brief Less than or equal operator, - /// where the value of _temp is assumed to be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return True if this is less than or equal to _temp. - public: bool operator<=(const double _temp) const; - - /// \brief Greater than operator - /// \param[in] _temp The temperature to compare - /// \return True if this is greater than _temp. - public: bool operator>(const Temperature &_temp) const; - - /// \brief Greater than operator, where the value of _temp is assumed to - /// be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return True if this is greater than _temp. - public: bool operator>(const double _temp) const; - - /// \brief Greater than or equal to operator - /// \param[in] _temp The temperature to compare - /// \return True if this is greater than or equal to _temp. - public: bool operator>=(const Temperature &_temp) const; - - /// \brief Greater than equal operator, - /// where the value of _temp is assumed to be in Kelvin - /// \param[in] _temp The temperature (in Kelvin) to compare - /// \return True if this is greater than or equal to _temp. - public: bool operator>=(const double _temp) const; - - /// \brief Stream insertion operator - /// \param[in] _out the output stream - /// \param[in] _temp Temperature to write to the stream - /// \return the output stream - public: friend std::ostream &operator<<(std::ostream &_out, - const ignition::math::Temperature &_temp) - { - _out << _temp.Kelvin(); - return _out; - } - - /// \brief Stream extraction operator - /// \param[in] _in the input stream - /// \param[in] _temp Temperature to read from to the stream. Assumes - /// temperature value is in Kelvin. - /// \return the input stream - public: friend std::istream &operator>>(std::istream &_in, - ignition::math::Temperature &_temp) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - - double kelvin; - _in >> kelvin; - - if (!_in.fail()) - { - _temp.SetKelvin(kelvin); - } - return _in; - } - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } - } -} -#endif diff --git a/include/ignition/math/Triangle.hh b/include/ignition/math/Triangle.hh index 6ad394ae0..9fd46e9cc 100644 --- a/include/ignition/math/Triangle.hh +++ b/include/ignition/math/Triangle.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Open Source Robotics Foundation + * 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. @@ -13,235 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_TRIANGLE_HH_ -#define IGNITION_MATH_TRIANGLE_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Triangle Triangle.hh ignition/math/Triangle.hh - /// \brief Triangle class and related functions. - template - class Triangle - { - /// \brief Default constructor - public: Triangle() = default; - - /// \brief Constructor - /// \param[in] _pt1 First point that defines the triangle. - /// \param[in] _pt2 Second point that defines the triangle. - /// \param[in] _pt3 Third point that defines the triangle. - public: Triangle(const math::Vector2 &_pt1, - const math::Vector2 &_pt2, - const math::Vector2 &_pt3) - { - this->Set(_pt1, _pt2, _pt3); - } - - /// \brief Set one vertex of the triangle. - /// \param[in] _index Index of the point to set, where - /// 0 == first vertex, 1 == second vertex, and 2 == third vertex. - /// The index is clamped to the range [0, 2]. - /// \param[in] _pt Value of the point to set. - public: void Set(const unsigned int _index, const math::Vector2 &_pt) - { - this->pts[clamp(_index, 0u, 2u)] = _pt; - } - - /// \brief Set all vertices of the triangle. - /// \param[in] _pt1 First point that defines the triangle. - /// \param[in] _pt2 Second point that defines the triangle. - /// \param[in] _pt3 Third point that defines the triangle. - public: void Set(const math::Vector2 &_pt1, - const math::Vector2 &_pt2, - const math::Vector2 &_pt3) - { - this->pts[0] = _pt1; - this->pts[1] = _pt2; - this->pts[2] = _pt3; - } - - /// \brief Get whether this triangle is valid, based on triangle - /// inequality: the sum of the lengths of any two sides must be greater - /// than the length of the remaining side. - /// \return True if the triangle inequality holds - public: bool Valid() const - { - T a = this->Side(0).Length(); - T b = this->Side(1).Length(); - T c = this->Side(2).Length(); - return (a+b) > c && (b+c) > a && (c+a) > b; - } - - /// \brief Get a line segment for one side of the triangle. - /// \param[in] _index Index of the side to retreive, where - /// 0 == Line2(pt1, pt2), - /// 1 == Line2(pt2, pt3), - /// 2 == Line2(pt3, pt1) - /// The index is clamped to the range [0, 2] - /// \return Line segment of the requested side. - public: Line2 Side(const unsigned int _index) const - { - if (_index == 0) - return Line2(this->pts[0], this->pts[1]); - else if (_index == 1) - return Line2(this->pts[1], this->pts[2]); - else - return Line2(this->pts[2], this->pts[0]); - } - - /// \brief Check if this triangle completely contains the given line - /// segment. - /// \param[in] _line Line to check. - /// \return True if the line's start and end points are both inside - /// this triangle. - public: bool Contains(const Line2 &_line) const - { - return this->Contains(_line[0]) && this->Contains(_line[1]); - } - - /// \brief Get whether this triangle contains the given point. - /// \param[in] _pt Point to check. - /// \return True if the point is inside or on the triangle. - public: bool Contains(const math::Vector2 &_pt) const - { - // Compute vectors - math::Vector2 v0 = this->pts[2] -this->pts[0]; - math::Vector2 v1 = this->pts[1] -this->pts[0]; - math::Vector2 v2 = _pt - this->pts[0]; - - // Compute dot products - double dot00 = v0.Dot(v0); - double dot01 = v0.Dot(v1); - double dot02 = v0.Dot(v2); - double dot11 = v1.Dot(v1); - double dot12 = v1.Dot(v2); - - // Compute barycentric coordinates - double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01); - double u = (dot11 * dot02 - dot01 * dot12) * invDenom; - double v = (dot00 * dot12 - dot01 * dot02) * invDenom; - - // Check if point is in triangle - return (u >= 0) && (v >= 0) && (u + v <= 1); - } - - /// \brief Get whether the given line intersects this triangle. - /// \param[in] _line Line to check. - /// \param[out] _ipt1 Return value of the first intersection point, - /// only valid if the return value of the function is true. - /// \param[out] _ipt2 Return value of the second intersection point, - /// only valid if the return value of the function is true. - /// \return True if the given line intersects this triangle. - public: bool Intersects(const Line2 &_line, - math::Vector2 &_ipt1, - math::Vector2 &_ipt2) const - { - if (this->Contains(_line)) - { - _ipt1 = _line[0]; - _ipt2 = _line[1]; - return true; - } - - Line2 line1(this->pts[0], this->pts[1]); - Line2 line2(this->pts[1], this->pts[2]); - Line2 line3(this->pts[2], this->pts[0]); - - math::Vector2 pt; - std::set > points; - - if (line1.Intersect(_line, pt)) - points.insert(pt); - - if (line2.Intersect(_line, pt)) - points.insert(pt); - - if (line3.Intersect(_line, pt)) - points.insert(pt); - - if (points.empty()) - { - return false; - } - else if (points.size() == 1) - { - auto iter = points.begin(); - - _ipt1 = *iter; - if (this->Contains(_line[0])) - _ipt2 = _line[0]; - else - { - _ipt2 = _line[1]; - } - } - else - { - auto iter = points.begin(); - _ipt1 = *(iter++); - _ipt2 = *iter; - } - - return true; - } - - /// \brief Get the length of the triangle's perimeter. - /// \return Sum of the triangle's line segments. - public: T Perimeter() const - { - return this->Side(0).Length() + this->Side(1).Length() + - this->Side(2).Length(); - } - - /// \brief Get the area of this triangle. - /// \return Triangle's area. - public: double Area() const - { - double s = this->Perimeter() / 2.0; - T a = this->Side(0).Length(); - T b = this->Side(1).Length(); - T c = this->Side(2).Length(); - - // Heron's formula - // http://en.wikipedia.org/wiki/Heron%27s_formula - return sqrt(s * (s-a) * (s-b) * (s-c)); - } - - /// \brief Get one of points that define the triangle. - /// \param[in] _index The index, where 0 == first vertex, - /// 1 == second vertex, and 2 == third vertex. - /// The index is clamped to the range [0, 2] - /// \return The point specified by _index. - public: math::Vector2 operator[](const size_t _index) const - { - return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; - } - - /// The points of the triangle - private: math::Vector2 pts[3]; - }; - - /// Integer specialization of the Triangle class. - typedef Triangle Trianglei; - - /// Double specialization of the Triangle class. - typedef Triangle Triangled; - - /// Float specialization of the Triangle class. - typedef Triangle Trianglef; - } - } -} -#endif diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index f68d5c187..423c1c04c 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 Open Source Robotics Foundation + * 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. @@ -13,274 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_TRIANGLE3_HH_ -#define IGNITION_MATH_TRIANGLE3_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Triangle3 Triangle3.hh ignition/math/Triangle3.hh - /// \brief A 3-dimensional triangle and related functions. - template - class Triangle3 - { - /// \brief Default constructor - public: Triangle3() = default; - - /// \brief Constructor. - /// - /// Keep in mind that the triangle normal - /// is determined by the order of these vertices. Search - /// the internet for "triangle winding" for more information. - /// \param[in] _pt1 First point that defines the triangle. - /// \param[in] _pt2 Second point that defines the triangle. - /// \param[in] _pt3 Third point that defines the triangle. - public: Triangle3(const Vector3 &_pt1, - const Vector3 &_pt2, - const Vector3 &_pt3) - { - this->Set(_pt1, _pt2, _pt3); - } - - /// \brief Set one vertex of the triangle. - /// - /// Keep in mind that the triangle normal - /// is determined by the order of these vertices. Search - /// the internet for "triangle winding" for more information. - /// - /// \param[in] _index Index of the point to set. _index is clamped - /// to the range [0,2]. - /// \param[in] _pt Value of the point to set. - public: void Set(const unsigned int _index, const Vector3 &_pt) - { - this->pts[clamp(_index, 0u, 2u)] = _pt; - } - - /// \brief Set all vertices of the triangle. - /// - /// Keep in mind that the triangle normal - /// is determined by the order of these vertices. Search - /// the internet for "triangle winding" for more information. - /// - /// \param[in] _pt1 First point that defines the triangle. - /// \param[in] _pt2 Second point that defines the triangle. - /// \param[in] _pt3 Third point that defines the triangle. - public: void Set(const Vector3 &_pt1, - const Vector3 &_pt2, - const Vector3 &_pt3) - { - this->pts[0] = _pt1; - this->pts[1] = _pt2; - this->pts[2] = _pt3; - } - - /// \brief Get whether this triangle is valid, based on triangle - /// inequality: the sum of the lengths of any two sides must be greater - /// than the length of the remaining side. - /// \return True if the triangle inequality holds - public: bool Valid() const - { - T a = this->Side(0).Length(); - T b = this->Side(1).Length(); - T c = this->Side(2).Length(); - return (a+b) > c && (b+c) > a && (c+a) > b; - } - - /// \brief Get a line segment for one side of the triangle. - /// \param[in] _index Index of the side to retrieve, where - /// 0 == Line3(pt1, pt2), - /// 1 == Line3(pt2, pt3), - /// 2 == Line3(pt3, pt1). - /// _index is clamped to the range [0,2]. - /// \return Line segment of the requested side. - public: Line3 Side(const unsigned int _index) const - { - if (_index == 0) - return Line3(this->pts[0], this->pts[1]); - else if (_index == 1) - return Line3(this->pts[1], this->pts[2]); - else - return Line3(this->pts[2], this->pts[0]); - } - - /// \brief Check if this triangle completely contains the given line - /// segment. - /// \param[in] _line Line to check. - /// \return True if the line's start and end points are both inside - /// this triangle. - public: bool Contains(const Line3 &_line) const - { - return this->Contains(_line[0]) && this->Contains(_line[1]); - } - - /// \brief Get whether this triangle contains the given point. - /// \param[in] _pt Point to check. - /// \return True if the point is inside or on the triangle. - public: bool Contains(const Vector3 &_pt) const - { - // Make sure the point is on the same plane as the triangle - if (Planed(this->Normal()).Side(Vector3d(_pt[0], _pt[1], _pt[2])) - == Planed::NO_SIDE) - { - Vector3 v0 = this->pts[2] - this->pts[0]; - Vector3 v1 = this->pts[1] - this->pts[0]; - Vector3 v2 = _pt - this->pts[0]; - - double dot00 = v0.Dot(v0); - double dot01 = v0.Dot(v1); - double dot02 = v0.Dot(v2); - double dot11 = v1.Dot(v1); - double dot12 = v1.Dot(v2); - - // Compute barycentric coordinates - double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01); - double u = (dot11 * dot02 - dot01 * dot12) * invDenom; - double v = (dot00 * dot12 - dot01 * dot02) * invDenom; - - // Check if point is in triangle - return (u >= 0) && (v >= 0) && (u + v <= 1); - } - return false; - } - - /// \brief Get the triangle's normal vector. - /// \return The normal vector for the triangle. - public: Vector3d Normal() const - { - return math::Vector3d::Normal( - Vector3d(this->pts[0][0], this->pts[0][1], this->pts[0][2]), - Vector3d(this->pts[1][0], this->pts[1][1], this->pts[1][2]), - Vector3d(this->pts[2][0], this->pts[2][1], this->pts[2][2])); - } - - /// \brief Get whether the given line intersects an edge of this triangle. - /// - /// The returned intersection point is one of: - /// - /// * If the line is coplanar with the triangle: - /// * The point on the closest edge of the triangle that the line - /// intersects. - /// OR - /// * The first point on the line, if the line is completely contained - /// * If the line is not coplanar, the point on the triangle that the - /// line intersects. - /// - /// \param[in] _line Line to check. - /// \param[out] _ipt1 Return value of the first intersection point, - /// only valid if the return value of the function is true. - /// \return True if the given line intersects this triangle. - public: bool Intersects( - const Line3 &_line, Vector3 &_ipt1) const - { - // Triangle normal - Vector3d norm = this->Normal(); - - // Ray direction to intersect with triangle - Vector3 dir = (_line[1] - _line[0]).Normalize(); - - double denom = norm.Dot(Vector3d(dir[0], dir[1], dir[2])); - - // Handle the case when the line is not co-planar with the triangle - if (!math::equal(denom, 0.0)) - { - // Distance from line start to triangle intersection - Vector3 diff = _line[0] - this->pts[0]; - double intersection = - -norm.Dot(Vector3d(diff[0], diff[1], diff[2])) / denom; - - // Make sure the ray intersects the triangle - if (intersection < 1.0 || intersection > _line.Length()) - return false; - - // Return point of intersection - _ipt1 = _line[0] + (dir * intersection); - - return true; - } - // Line co-planar with triangle - else - { - // If the line is completely inside the triangle - if (this->Contains(_line)) - { - _ipt1 = _line[0]; - return true; - } - // If the line intersects the first side - else if (_line.Intersect(this->Side(0), _ipt1)) - { - return true; - } - // If the line intersects the second side - else if (_line.Intersect(this->Side(1), _ipt1)) - { - return true; - } - // If the line intersects the third side - else if (_line.Intersect(this->Side(2), _ipt1)) - { - return true; - } - } - - return false; - } - - /// \brief Get the length of the triangle's perimeter. - /// \return Sum of the triangle's line segments. - public: T Perimeter() const - { - return this->Side(0).Length() + this->Side(1).Length() + - this->Side(2).Length(); - } - - /// \brief Get the area of this triangle. - /// \return Triangle's area. - public: double Area() const - { - double s = this->Perimeter() / 2.0; - T a = this->Side(0).Length(); - T b = this->Side(1).Length(); - T c = this->Side(2).Length(); - - // Heron's formula - // http://en.wikipedia.org/wiki/Heron%27s_formula - return sqrt(s * (s-a) * (s-b) * (s-c)); - } - - /// \brief Get one of points that define the triangle. - /// \param[in] _index: 0, 1, or 2. _index is clamped to the range - /// [0,2]. - /// \return The triangle point at _index. - public: Vector3 operator[](const size_t _index) const - { - return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; - } - - /// The points of the triangle - private: Vector3 pts[3]; - }; - - /// \brief Integer specialization of the Triangle class. - typedef Triangle3 Triangle3i; - - /// \brief Double specialization of the Triangle class. - typedef Triangle3 Triangle3d; - - /// \brief Float specialization of the Triangle class. - typedef Triangle3 Triangle3f; - } - } -} -#endif diff --git a/include/ignition/math/Vector2.hh b/include/ignition/math/Vector2.hh index 6bf68b1d0..71f4d6861 100644 --- a/include/ignition/math/Vector2.hh +++ b/include/ignition/math/Vector2.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,586 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_VECTOR2_HH_ -#define IGNITION_MATH_VECTOR2_HH_ + */ -#include -#include -#include - -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Vector2 Vector2.hh ignition/math/Vector2.hh - /// \brief Two dimensional (x, y) vector. - template - class Vector2 - { - /// \brief math::Vector2(0, 0) - public: static const Vector2 Zero; - - /// \brief math::Vector2(1, 1) - public: static const Vector2 One; - - /// \brief math::Vector2(NaN, NaN, NaN) - public: static const Vector2 NaN; - - /// \brief Default Constructor - public: Vector2() - { - this->data[0] = 0; - this->data[1] = 0; - } - - /// \brief Constructor - /// \param[in] _x value along x - /// \param[in] _y value along y - public: Vector2(const T &_x, const T &_y) - { - this->data[0] = _x; - this->data[1] = _y; - } - - /// \brief Copy constructor - /// \param[in] _v the value - public: Vector2(const Vector2 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - } - - /// \brief Destructor - public: virtual ~Vector2() {} - - /// \brief Return the sum of the values - /// \return the sum - public: T Sum() const - { - return this->data[0] + this->data[1]; - } - - /// \brief Calc distance to the given point - /// \param[in] _pt The point to measure to - /// \return the distance - public: double Distance(const Vector2 &_pt) const - { - return sqrt((this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + - (this->data[1]-_pt[1])*(this->data[1]-_pt[1])); - } - - /// \brief Returns the length (magnitude) of the vector - /// \return The length - public: T Length() const - { - return static_cast(sqrt(this->SquaredLength())); - } - - /// \brief Returns the square of the length (magnitude) of the vector - /// \return The squared length - public: T SquaredLength() const - { - return - this->data[0] * this->data[0] + - this->data[1] * this->data[1]; - } - - /// \brief Normalize the vector length - public: void Normalize() - { - T d = this->Length(); - - if (!equal(d, static_cast(0.0))) - { - this->data[0] /= d; - this->data[1] /= d; - } - } - - /// \brief Returns a normalized vector - /// \return unit length vector - public: Vector2 Normalized() const - { - Vector2 result = *this; - result.Normalize(); - return result; - } - - /// \brief Round to near whole number, return the result. - /// \return the result - public: Vector2 Round() - { - this->data[0] = static_cast(std::nearbyint(this->data[0])); - this->data[1] = static_cast(std::nearbyint(this->data[1])); - return *this; - } - - /// \brief Get a rounded version of this vector - /// \return a rounded vector - public: Vector2 Rounded() const - { - Vector2 result = *this; - result.Round(); - return result; - } - - /// \brief Set the contents of the vector - /// \param[in] _x value along x - /// \param[in] _y value along y - public: void Set(T _x, T _y) - { - this->data[0] = _x; - this->data[1] = _y; - } - - /// \brief Get the dot product of this vector and _v - /// \param[in] _v the vector - /// \return The dot product - public: T Dot(const Vector2 &_v) const - { - return (this->data[0] * _v[0]) + (this->data[1] * _v[1]); - } - - /// \brief Get the absolute value of the vector - /// \return a vector with positive elements - public: Vector2 Abs() const - { - return Vector2(std::abs(this->data[0]), - std::abs(this->data[1])); - } - - /// \brief Return the absolute dot product of this vector and - /// another vector. This is similar to the Dot function, except the - /// absolute value of each component of the vector is used. - /// - /// result = abs(x1 * x2) + abs(y1 * y2) - /// - /// \param[in] _v The vector - /// \return The absolute dot product - public: T AbsDot(const Vector2 &_v) const - { - return std::abs(this->data[0] * _v[0]) + - std::abs(this->data[1] * _v[1]); - } - - /// \brief Corrects any nan values - public: inline void Correct() - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - if (!std::isfinite(static_cast(this->data[0]))) - this->data[0] = 0; - if (!std::isfinite(static_cast(this->data[1]))) - this->data[1] = 0; - } - - /// \brief Set this vector's components to the maximum of itself and the - /// passed in vector - /// \param[in] _v the maximum clamping vector - public: void Max(const Vector2 &_v) - { - this->data[0] = std::max(_v[0], this->data[0]); - this->data[1] = std::max(_v[1], this->data[1]); - } - - /// \brief Set this vector's components to the minimum of itself and the - /// passed in vector - /// \param[in] _v the minimum clamping vector - public: void Min(const Vector2 &_v) - { - this->data[0] = std::min(_v[0], this->data[0]); - this->data[1] = std::min(_v[1], this->data[1]); - } - - /// \brief Get the maximum value in the vector - /// \return the maximum element - public: T Max() const - { - return std::max(this->data[0], this->data[1]); - } - - /// \brief Get the minimum value in the vector - /// \return the minimum element - public: T Min() const - { - return std::min(this->data[0], this->data[1]); - } - - /// \brief Assignment operator - /// \param[in] _v a value for x and y element - /// \return this - public: Vector2 &operator=(const Vector2 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - - return *this; - } - - /// \brief Assignment operator - /// \param[in] _v the value for x and y element - /// \return this - public: const Vector2 &operator=(T _v) - { - this->data[0] = _v; - this->data[1] = _v; - - return *this; - } - - /// \brief Addition operator - /// \param[in] _v vector to add - /// \return sum vector - public: Vector2 operator+(const Vector2 &_v) const - { - return Vector2(this->data[0] + _v[0], this->data[1] + _v[1]); - } - - /// \brief Addition assignment operator - /// \param[in] _v the vector to add - // \return this - public: const Vector2 &operator+=(const Vector2 &_v) - { - this->data[0] += _v[0]; - this->data[1] += _v[1]; - - return *this; - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \return sum vector - public: inline Vector2 operator+(const T _s) const - { - return Vector2(this->data[0] + _s, - this->data[1] + _s); - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \param[in] _v input vector - /// \return sum vector - public: friend inline Vector2 operator+(const T _s, - const Vector2 &_v) - { - return _v + _s; - } - - /// \brief Addition assignment operator - /// \param[in] _s scalar addend - /// \return this - public: const Vector2 &operator+=(const T _s) - { - this->data[0] += _s; - this->data[1] += _s; - - return *this; - } - - /// \brief Negation operator - /// \return negative of this vector - public: inline Vector2 operator-() const - { - return Vector2(-this->data[0], -this->data[1]); - } - - /// \brief Subtraction operator - /// \param[in] _v the vector to substract - /// \return the subtracted vector - public: Vector2 operator-(const Vector2 &_v) const - { - return Vector2(this->data[0] - _v[0], this->data[1] - _v[1]); - } - - /// \brief Subtraction assignment operator - /// \param[in] _v the vector to substract - /// \return this - public: const Vector2 &operator-=(const Vector2 &_v) - { - this->data[0] -= _v[0]; - this->data[1] -= _v[1]; - - return *this; - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar subtrahend - /// \return difference vector - public: inline Vector2 operator-(const T _s) const - { - return Vector2(this->data[0] - _s, - this->data[1] - _s); - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar minuend - /// \param[in] _v vector subtrahend - /// \return difference vector - public: friend inline Vector2 operator-(const T _s, - const Vector2 &_v) - { - return {_s - _v.X(), _s - _v.Y()}; - } - - /// \brief Subtraction assignment operator - /// \param[in] _s scalar subtrahend - /// \return this - public: const Vector2 &operator-=(T _s) - { - this->data[0] -= _s; - this->data[1] -= _s; - - return *this; - } - - /// \brief Division operator - /// \remarks this is an element wise division - /// \param[in] _v a vector - /// \result a result - public: const Vector2 operator/(const Vector2 &_v) const - { - return Vector2(this->data[0] / _v[0], this->data[1] / _v[1]); - } - - /// \brief Division operator - /// \remarks this is an element wise division - /// \param[in] _v a vector - /// \return this - public: const Vector2 &operator/=(const Vector2 &_v) - { - this->data[0] /= _v[0]; - this->data[1] /= _v[1]; - - return *this; - } - - /// \brief Division operator - /// \param[in] _v the value - /// \return a vector - public: const Vector2 operator/(T _v) const - { - return Vector2(this->data[0] / _v, this->data[1] / _v); - } - - /// \brief Division operator - /// \param[in] _v the divisor - /// \return a vector - public: const Vector2 &operator/=(T _v) - { - this->data[0] /= _v; - this->data[1] /= _v; - - return *this; - } - - /// \brief Multiplication operators - /// \param[in] _v the vector - /// \return the result - public: const Vector2 operator*(const Vector2 &_v) const - { - return Vector2(this->data[0] * _v[0], this->data[1] * _v[1]); - } - - /// \brief Multiplication assignment operator - /// \remarks this is an element wise multiplication - /// \param[in] _v the vector - /// \return this - public: const Vector2 &operator*=(const Vector2 &_v) - { - this->data[0] *= _v[0]; - this->data[1] *= _v[1]; - - return *this; - } - - /// \brief Multiplication operators - /// \param[in] _v the scaling factor - /// \return a scaled vector - public: const Vector2 operator*(T _v) const - { - return Vector2(this->data[0] * _v, this->data[1] * _v); - } - - /// \brief Scalar left multiplication operators - /// \param[in] _s the scaling factor - /// \param[in] _v the vector to scale - /// \return a scaled vector - public: friend inline const Vector2 operator*(const T _s, - const Vector2 &_v) - { - return Vector2(_v * _s); - } - - /// \brief Multiplication assignment operator - /// \param[in] _v the scaling factor - /// \return a scaled vector - public: const Vector2 &operator*=(T _v) - { - this->data[0] *= _v; - this->data[1] *= _v; - - return *this; - } - - /// \brief Equality test with tolerance. - /// \param[in] _v the vector to compare to - /// \param[in] _tol equality tolerance. - /// \return true if the elements of the vectors are equal within - /// the tolerence specified by _tol. - public: bool Equal(const Vector2 &_v, const T &_tol) const - { - return equal(this->data[0], _v[0], _tol) - && equal(this->data[1], _v[1], _tol); - } - - /// \brief Equal to operator - /// \param[in] _v the vector to compare to - /// \return true if the elements of the 2 vectors are equal within - /// a tolerence (1e-6) - public: bool operator==(const Vector2 &_v) const - { - return this->Equal(_v, static_cast(1e-6)); - } - - /// \brief Not equal to operator - /// \return true if elements are of diffent values (tolerence 1e-6) - public: bool operator!=(const Vector2 &_v) const - { - return !(*this == _v); - } - - /// \brief See if a point is finite (e.g., not nan) - /// \return true if finite, false otherwise - public: bool IsFinite() const - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - return std::isfinite(static_cast(this->data[0])) && - std::isfinite(static_cast(this->data[1])); - } - - /// \brief Array subscript operator - /// \param[in] _index The index, where 0 == x and 1 == y. - /// The index is clamped to the range [0,1]. - public: T &operator[](const std::size_t _index) - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; - } - - /// \brief Const-qualified array subscript operator - /// \param[in] _index The index, where 0 == x and 1 == y. - /// The index is clamped to the range [0,1]. - public: T operator[](const std::size_t _index) const - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)]; - } - - /// \brief Return the x value. - /// \return Value of the X component. - public: inline T X() const - { - return this->data[0]; - } - - /// \brief Return the y value. - /// \return Value of the Y component. - public: inline T Y() const - { - return this->data[1]; - } - - /// \brief Return a mutable x value. - /// \return Value of the X component. - public: inline T &X() - { - return this->data[0]; - } - - /// \brief Return a mutable y value. - /// \return Value of the Y component. - public: inline T &Y() - { - return this->data[1]; - } - - /// \brief Set the x value. - /// \param[in] _v Value for the x component. - public: inline void X(const T &_v) - { - this->data[0] = _v; - } - - /// \brief Set the y value. - /// \param[in] _v Value for the y component. - public: inline void Y(const T &_v) - { - this->data[1] = _v; - } - - /// \brief Stream extraction operator - /// \param[in] _out output stream - /// \param[in] _pt Vector2 to output - /// \return The stream - public: friend std::ostream - &operator<<(std::ostream &_out, const Vector2 &_pt) - { - _out << _pt[0] << " " << _pt[1]; - return _out; - } - - /// \brief Less than operator. - /// \param[in] _pt Vector to compare. - /// \return True if this vector's first or second value is less than - /// the given vector's first or second value. - public: bool operator<(const Vector2 &_pt) const - { - return this->data[0] < _pt[0] || this->data[1] < _pt[1]; - } - - /// \brief Stream extraction operator - /// \param[in] _in input stream - /// \param[in] _pt Vector2 to read values into - /// \return The stream - public: friend std::istream - &operator>>(std::istream &_in, Vector2 &_pt) - { - T x, y; - // Skip white spaces - _in.setf(std::ios_base::skipws); - _in >> x >> y; - if (!_in.fail()) - { - _pt.Set(x, y); - } - return _in; - } - - /// \brief The x and y values. - private: T data[2]; - }; - - template - const Vector2 Vector2::Zero(0, 0); - - template - const Vector2 Vector2::One(1, 1); - - template - const Vector2 Vector2::NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - - typedef Vector2 Vector2i; - typedef Vector2 Vector2d; - typedef Vector2 Vector2f; - } - } -} -#endif diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 85952e13a..b8bbc8965 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Open Source Robotics Foundation + * 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. @@ -13,763 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_VECTOR3_HH_ -#define IGNITION_MATH_VECTOR3_HH_ + */ -#include -#include -#include -#include -#include - -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Vector3 Vector3.hh ignition/math/Vector3.hh - /// \brief The Vector3 class represents the generic vector containing 3 - /// elements. Since it's commonly used to keep coordinate system - /// related information, its elements are labeled by x, y, z. - template - class Vector3 - { - /// \brief math::Vector3(0, 0, 0) - public: static const Vector3 Zero; - - /// \brief math::Vector3(1, 1, 1) - public: static const Vector3 One; - - /// \brief math::Vector3(1, 0, 0) - public: static const Vector3 UnitX; - - /// \brief math::Vector3(0, 1, 0) - public: static const Vector3 UnitY; - - /// \brief math::Vector3(0, 0, 1) - public: static const Vector3 UnitZ; - - /// \brief math::Vector3(NaN, NaN, NaN) - public: static const Vector3 NaN; - - /// \brief Constructor - public: Vector3() - { - this->data[0] = 0; - this->data[1] = 0; - this->data[2] = 0; - } - - /// \brief Constructor - /// \param[in] _x value along x - /// \param[in] _y value along y - /// \param[in] _z value along z - public: Vector3(const T &_x, const T &_y, const T &_z) - { - this->data[0] = _x; - this->data[1] = _y; - this->data[2] = _z; - } - - /// \brief Copy constructor - /// \param[in] _v a vector - public: Vector3(const Vector3 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - this->data[2] = _v[2]; - } - - /// \brief Destructor - public: virtual ~Vector3() {} - - /// \brief Return the sum of the values - /// \return the sum - public: T Sum() const - { - return this->data[0] + this->data[1] + this->data[2]; - } - - /// \brief Calc distance to the given point - /// \param[in] _pt the point - /// \return the distance - public: T Distance(const Vector3 &_pt) const - { - return static_cast(sqrt( - (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + - (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) + - (this->data[2]-_pt[2])*(this->data[2]-_pt[2]))); - } - - /// \brief Calc distance to the given point - /// \param[in] _x value along x - /// \param[in] _y value along y - /// \param[in] _z value along z - /// \return the distance - public: T Distance(T _x, T _y, T _z) const - { - return this->Distance(Vector3(_x, _y, _z)); - } - - /// \brief Returns the length (magnitude) of the vector - /// \return the length - public: T Length() const - { - return static_cast(sqrt(this->SquaredLength())); - } - - /// \brief Return the square of the length (magnitude) of the vector - /// \return the squared length - public: T SquaredLength() const - { - return - this->data[0] * this->data[0] + - this->data[1] * this->data[1] + - this->data[2] * this->data[2]; - } - - /// \brief Normalize the vector length - /// \return unit length vector - public: Vector3 Normalize() - { - T d = this->Length(); - - if (!equal(d, static_cast(0.0))) - { - this->data[0] /= d; - this->data[1] /= d; - this->data[2] /= d; - } - - return *this; - } - - /// \brief Return a normalized vector - /// \return unit length vector - public: Vector3 Normalized() const - { - Vector3 result = *this; - result.Normalize(); - return result; - } - - /// \brief Round to near whole number, return the result. - /// \return the result - public: Vector3 Round() - { - this->data[0] = static_cast(std::nearbyint(this->data[0])); - this->data[1] = static_cast(std::nearbyint(this->data[1])); - this->data[2] = static_cast(std::nearbyint(this->data[2])); - return *this; - } - - /// \brief Get a rounded version of this vector - /// \return a rounded vector - public: Vector3 Rounded() const - { - Vector3 result = *this; - result.Round(); - return result; - } - - /// \brief Set the contents of the vector - /// \param[in] _x value along x - /// \param[in] _y value along y - /// \param[in] _z value aling z - public: inline void Set(T _x = 0, T _y = 0, T _z = 0) - { - this->data[0] = _x; - this->data[1] = _y; - this->data[2] = _z; - } - - /// \brief Return the cross product of this vector with another vector. - /// \param[in] _v a vector - /// \return the cross product - public: Vector3 Cross(const Vector3 &_v) const - { - return Vector3(this->data[1] * _v[2] - this->data[2] * _v[1], - this->data[2] * _v[0] - this->data[0] * _v[2], - this->data[0] * _v[1] - this->data[1] * _v[0]); - } - - /// \brief Return the dot product of this vector and another vector - /// \param[in] _v the vector - /// \return the dot product - public: T Dot(const Vector3 &_v) const - { - return this->data[0] * _v[0] + - this->data[1] * _v[1] + - this->data[2] * _v[2]; - } - - /// \brief Return the absolute dot product of this vector and - /// another vector. This is similar to the Dot function, except the - /// absolute value of each component of the vector is used. - /// - /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 *z2) - /// - /// \param[in] _v the vector - /// \return The absolute dot product - public: T AbsDot(const Vector3 &_v) const - { - return std::abs(this->data[0] * _v[0]) + - std::abs(this->data[1] * _v[1]) + - std::abs(this->data[2] * _v[2]); - } - - /// \brief Get the absolute value of the vector - /// \return a vector with positive elements - public: Vector3 Abs() const - { - return Vector3(std::abs(this->data[0]), - std::abs(this->data[1]), - std::abs(this->data[2])); - } - - /// \brief Return a vector that is perpendicular to this one. - /// \return an orthogonal vector - public: Vector3 Perpendicular() const - { - static const T sqrZero = static_cast(1e-06 * 1e-06); - - Vector3 perp = this->Cross(Vector3(1, 0, 0)); - - // Check the length of the vector - if (perp.SquaredLength() < sqrZero) - { - perp = this->Cross(Vector3(0, 1, 0)); - } - - return perp; - } - - /// \brief Get a normal vector to a triangle - /// \param[in] _v1 first vertex of the triangle - /// \param[in] _v2 second vertex - /// \param[in] _v3 third vertex - /// \return the normal - public: static Vector3 Normal(const Vector3 &_v1, - const Vector3 &_v2, const Vector3 &_v3) - { - Vector3 a = _v2 - _v1; - Vector3 b = _v3 - _v1; - Vector3 n = a.Cross(b); - return n.Normalize(); - } - - /// \brief Get distance to an infinite line defined by 2 points. - /// \param[in] _pt1 first point on the line - /// \param[in] _pt2 second point on the line - /// \return the minimum distance from this point to the line - public: T DistToLine(const Vector3 &_pt1, const Vector3 &_pt2) - { - T d = ((*this) - _pt1).Cross((*this) - _pt2).Length(); - d = d / (_pt2 - _pt1).Length(); - return d; - } - - /// \brief Set this vector's components to the maximum of itself and the - /// passed in vector - /// \param[in] _v the maximum clamping vector - public: void Max(const Vector3 &_v) - { - if (_v[0] > this->data[0]) - this->data[0] = _v[0]; - if (_v[1] > this->data[1]) - this->data[1] = _v[1]; - if (_v[2] > this->data[2]) - this->data[2] = _v[2]; - } - - /// \brief Set this vector's components to the minimum of itself and the - /// passed in vector - /// \param[in] _v the minimum clamping vector - public: void Min(const Vector3 &_v) - { - if (_v[0] < this->data[0]) - this->data[0] = _v[0]; - if (_v[1] < this->data[1]) - this->data[1] = _v[1]; - if (_v[2] < this->data[2]) - this->data[2] = _v[2]; - } - - /// \brief Get the maximum value in the vector - /// \return the maximum element - public: T Max() const - { - return std::max(std::max(this->data[0], this->data[1]), this->data[2]); - } - - /// \brief Get the minimum value in the vector - /// \return the minimum element - public: T Min() const - { - return std::min(std::min(this->data[0], this->data[1]), this->data[2]); - } - - /// \brief Assignment operator - /// \param[in] _v a new value - /// \return this - public: Vector3 &operator=(const Vector3 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - this->data[2] = _v[2]; - - return *this; - } - - /// \brief Assignment operator - /// \param[in] _v assigned to all elements - /// \return this - public: Vector3 &operator=(T _v) - { - this->data[0] = _v; - this->data[1] = _v; - this->data[2] = _v; - - return *this; - } - - /// \brief Addition operator - /// \param[in] _v vector to add - /// \return the sum vector - public: Vector3 operator+(const Vector3 &_v) const - { - return Vector3(this->data[0] + _v[0], - this->data[1] + _v[1], - this->data[2] + _v[2]); - } - - /// \brief Addition assignment operator - /// \param[in] _v vector to add - /// \return the sum vector - public: const Vector3 &operator+=(const Vector3 &_v) - { - this->data[0] += _v[0]; - this->data[1] += _v[1]; - this->data[2] += _v[2]; - - return *this; - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \return sum vector - public: inline Vector3 operator+(const T _s) const - { - return Vector3(this->data[0] + _s, - this->data[1] + _s, - this->data[2] + _s); - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \param[in] _v input vector - /// \return sum vector - public: friend inline Vector3 operator+(const T _s, - const Vector3 &_v) - { - return {_v.X() + _s, _v.Y() + _s, _v.Z() + _s}; - } - - /// \brief Addition assignment operator - /// \param[in] _s scalar addend - /// \return this - public: const Vector3 &operator+=(const T _s) - { - this->data[0] += _s; - this->data[1] += _s; - this->data[2] += _s; - - return *this; - } - - /// \brief Negation operator - /// \return negative of this vector - public: inline Vector3 operator-() const - { - return Vector3(-this->data[0], -this->data[1], -this->data[2]); - } - - /// \brief Subtraction operators - /// \param[in] _pt a vector to substract - /// \return a vector after the substraction - public: inline Vector3 operator-(const Vector3 &_pt) const - { - return Vector3(this->data[0] - _pt[0], - this->data[1] - _pt[1], - this->data[2] - _pt[2]); - } - - /// \brief Subtraction assignment operators - /// \param[in] _pt subtrahend - /// \return a vector after the substraction - public: const Vector3 &operator-=(const Vector3 &_pt) - { - this->data[0] -= _pt[0]; - this->data[1] -= _pt[1]; - this->data[2] -= _pt[2]; - - return *this; - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar subtrahend - /// \return difference vector - public: inline Vector3 operator-(const T _s) const - { - return Vector3(this->data[0] - _s, - this->data[1] - _s, - this->data[2] - _s); - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar minuend - /// \param[in] _v vector subtrahend - /// \return difference vector - public: friend inline Vector3 operator-(const T _s, - const Vector3 &_v) - { - return {_s - _v.X(), _s - _v.Y(), _s - _v.Z()}; - } - - /// \brief Subtraction assignment operator - /// \param[in] _s scalar subtrahend - /// \return this - public: const Vector3 &operator-=(const T _s) - { - this->data[0] -= _s; - this->data[1] -= _s; - this->data[2] -= _s; - - return *this; - } - - /// \brief Division operator - /// \remarks this is an element wise division - /// \param[in] _pt the vector divisor - /// \return a vector - public: const Vector3 operator/(const Vector3 &_pt) const - { - return Vector3(this->data[0] / _pt[0], - this->data[1] / _pt[1], - this->data[2] / _pt[2]); - } - - /// \brief Division assignment operator - /// \remarks this is an element wise division - /// \param[in] _pt the vector divisor - /// \return a vector - public: const Vector3 &operator/=(const Vector3 &_pt) - { - this->data[0] /= _pt[0]; - this->data[1] /= _pt[1]; - this->data[2] /= _pt[2]; - - return *this; - } - - /// \brief Division operator - /// \remarks this is an element wise division - /// \param[in] _v the divisor - /// \return a vector - public: const Vector3 operator/(T _v) const - { - return Vector3(this->data[0] / _v, - this->data[1] / _v, - this->data[2] / _v); - } - - /// \brief Division assignment operator - /// \remarks this is an element wise division - /// \param[in] _v the divisor - /// \return this - public: const Vector3 &operator/=(T _v) - { - this->data[0] /= _v; - this->data[1] /= _v; - this->data[2] /= _v; - - return *this; - } - - /// \brief Multiplication operator - /// \remarks this is an element wise multiplication, not a cross product - /// \param[in] _p multiplier operator - /// \return a vector - public: Vector3 operator*(const Vector3 &_p) const - { - return Vector3(this->data[0] * _p[0], - this->data[1] * _p[1], - this->data[2] * _p[2]); - } - - /// \brief Multiplication assignment operators - /// \remarks this is an element wise multiplication, not a cross product - /// \param[in] _v a vector - /// \return this - public: const Vector3 &operator*=(const Vector3 &_v) - { - this->data[0] *= _v[0]; - this->data[1] *= _v[1]; - this->data[2] *= _v[2]; - - return *this; - } - - /// \brief Multiplication operators - /// \param[in] _s the scaling factor - /// \return a scaled vector - public: inline Vector3 operator*(T _s) const - { - return Vector3(this->data[0] * _s, - this->data[1] * _s, - this->data[2] * _s); - } - - /// \brief Multiplication operators - /// \param[in] _s the scaling factor - /// \param[in] _v input vector - /// \return a scaled vector - public: friend inline Vector3 operator*(T _s, const Vector3 &_v) - { - return {_v.X() * _s, _v.Y() * _s, _v.Z() * _s}; - } - - /// \brief Multiplication operator - /// \param[in] _v scaling factor - /// \return this - public: const Vector3 &operator*=(T _v) - { - this->data[0] *= _v; - this->data[1] *= _v; - this->data[2] *= _v; - - return *this; - } - - /// \brief Equality test with tolerance. - /// \param[in] _v the vector to compare to - /// \param[in] _tol equality tolerance. - /// \return true if the elements of the vectors are equal within - /// the tolerence specified by _tol. - public: bool Equal(const Vector3 &_v, const T &_tol) const - { - return equal(this->data[0], _v[0], _tol) - && equal(this->data[1], _v[1], _tol) - && equal(this->data[2], _v[2], _tol); - } - - /// \brief Equal to operator - /// \param[in] _v The vector to compare against - /// \return true if each component is equal within a - /// default tolerence (1e-3), false otherwise - public: bool operator==(const Vector3 &_v) const - { - return this->Equal(_v, static_cast(1e-3)); - } - - /// \brief Not equal to operator - /// \param[in] _v The vector to compare against - /// \return false if each component is equal within a - /// default tolerence (1e-3), true otherwise - public: bool operator!=(const Vector3 &_v) const - { - return !(*this == _v); - } - - /// \brief See if a point is finite (e.g., not nan) - /// \return true if is finite or false otherwise - public: bool IsFinite() const - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - return std::isfinite(static_cast(this->data[0])) && - std::isfinite(static_cast(this->data[1])) && - std::isfinite(static_cast(this->data[2])); - } - - /// \brief Corrects any nan values - public: inline void Correct() - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - if (!std::isfinite(static_cast(this->data[0]))) - this->data[0] = 0; - if (!std::isfinite(static_cast(this->data[1]))) - this->data[1] = 0; - if (!std::isfinite(static_cast(this->data[2]))) - this->data[2] = 0; - } - - /// \brief Array subscript operator - /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z. - /// The index is clamped to the range [0,2]. - /// \return The value. - public: T &operator[](const std::size_t _index) - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; - } - - /// \brief Const-qualified array subscript operator - /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z. - /// The index is clamped to the range [0,2]. - /// \return The value. - public: T operator[](const std::size_t _index) const - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]; - } - - /// \brief Round all values to _precision decimal places - /// \param[in] _precision the decimal places - public: void Round(int _precision) - { - this->data[0] = precision(this->data[0], _precision); - this->data[1] = precision(this->data[1], _precision); - this->data[2] = precision(this->data[2], _precision); - } - - /// \brief Equality test - /// \remarks This is equivalent to the == operator - /// \param[in] _v the other vector - /// \return true if the 2 vectors have the same values, false otherwise - public: bool Equal(const Vector3 &_v) const - { - return equal(this->data[0], _v[0]) && - equal(this->data[1], _v[1]) && - equal(this->data[2], _v[2]); - } - - /// \brief Get the x value. - /// \return The x component of the vector - public: inline T X() const - { - return this->data[0]; - } - - /// \brief Get the y value. - /// \return The y component of the vector - public: inline T Y() const - { - return this->data[1]; - } - - /// \brief Get the z value. - /// \return The z component of the vector - public: inline T Z() const - { - return this->data[2]; - } - - /// \brief Get a mutable reference to the x value. - /// \return The x component of the vector - public: inline T &X() - { - return this->data[0]; - } - - /// \brief Get a mutable reference to the y value. - /// \return The y component of the vector - public: inline T &Y() - { - return this->data[1]; - } - - /// \brief Get a mutable reference to the z value. - /// \return The z component of the vector - public: inline T &Z() - { - return this->data[2]; - } - - /// \brief Set the x value. - /// \param[in] _v Value for the x component. - public: inline void X(const T &_v) - { - this->data[0] = _v; - } - - /// \brief Set the y value. - /// \param[in] _v Value for the y component. - public: inline void Y(const T &_v) - { - this->data[1] = _v; - } - - /// \brief Set the z value. - /// \param[in] _v Value for the z component. - public: inline void Z(const T &_v) - { - this->data[2] = _v; - } - - /// \brief Less than operator. - /// \param[in] _pt Vector to compare. - /// \return True if this vector's X(), Y(), or Z() value is less - /// than the given vector's corresponding values. - public: bool operator<(const Vector3 &_pt) const - { - return this->data[0] < _pt[0] || this->data[1] < _pt[1] || - this->data[2] < _pt[2]; - } - - /// \brief Stream insertion operator - /// \param _out output stream - /// \param _pt Vector3 to output - /// \return the stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Vector3 &_pt) - { - _out << precision(_pt[0], 6) << " " << precision(_pt[1], 6) << " " - << precision(_pt[2], 6); - return _out; - } - - /// \brief Stream extraction operator - /// \param _in input stream - /// \param _pt vector3 to read values into - /// \return the stream - public: friend std::istream &operator>>( - std::istream &_in, ignition::math::Vector3 &_pt) - { - // Skip white spaces - _in.setf(std::ios_base::skipws); - T x, y, z; - _in >> x >> y >> z; - if (!_in.fail()) - { - _pt.Set(x, y, z); - } - return _in; - } - - /// \brief The x, y, and z values - private: T data[3]; - }; - - template const Vector3 Vector3::Zero(0, 0, 0); - template const Vector3 Vector3::One(1, 1, 1); - template const Vector3 Vector3::UnitX(1, 0, 0); - template const Vector3 Vector3::UnitY(0, 1, 0); - template const Vector3 Vector3::UnitZ(0, 0, 1); - template const Vector3 Vector3::NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - - typedef Vector3 Vector3i; - typedef Vector3 Vector3d; - typedef Vector3 Vector3f; - } - } -} -#endif diff --git a/include/ignition/math/Vector3Stats.hh b/include/ignition/math/Vector3Stats.hh index 65d45ba45..af30f152d 100644 --- a/include/ignition/math/Vector3Stats.hh +++ b/include/ignition/math/Vector3Stats.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Open Source Robotics Foundation + * 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. @@ -13,100 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_VECTOR3STATS_HH_ -#define IGNITION_MATH_VECTOR3STATS_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \brief Forward declare private data class. - class Vector3StatsPrivate; - - /// \class Vector3Stats Vector3Stats.hh ignition/math/Vector3Stats.hh - /// \brief Collection of statistics for a Vector3 signal. - class IGNITION_MATH_VISIBLE Vector3Stats - { - /// \brief Constructor - public: Vector3Stats(); - - /// \brief Destructor - public: ~Vector3Stats(); - - /// \brief Add a new sample to the statistical measures. - /// \param[in] _data New signal data point. - public: void InsertData(const Vector3d &_data); - - /// \brief Add a new type of statistic. - /// \param[in] _name Short name of new statistic. - /// Valid values include: - /// "maxAbs" - /// "mean" - /// "rms" - /// \return True if statistic was successfully added, - /// false if name was not recognized or had already - /// been inserted. - public: bool InsertStatistic(const std::string &_name); - - /// \brief Add multiple statistics. - /// \param[in] _names Comma-separated list of new statistics. - /// For example, all statistics could be added with: - /// "maxAbs,mean,rms" - /// \return True if all statistics were successfully added, - /// false if any names were not recognized or had already - /// been inserted. - public: bool InsertStatistics(const std::string &_names); - - /// \brief Forget all previous data. - public: void Reset(); - - /// \brief Get statistics for x component of signal. - /// \return Statistics for x component of signal. - public: const SignalStats &X() const; - - /// \brief Get statistics for y component of signal. - /// \return Statistics for y component of signal. - public: const SignalStats &Y() const; - - /// \brief Get statistics for z component of signal. - /// \return Statistics for z component of signal. - public: const SignalStats &Z() const; - - /// \brief Get statistics for magnitude component of signal. - /// \return Statistics for magnitude component of signal. - public: const SignalStats &Mag() const; - - /// \brief Get mutable reference to statistics for x component of signal. - /// \return Statistics for x component of signal. - public: SignalStats &X(); - - /// \brief Get mutable reference to statistics for y component of signal. - /// \return Statistics for y component of signal. - public: SignalStats &Y(); - - /// \brief Get mutable reference to statistics for z component of signal. - /// \return Statistics for z component of signal. - public: SignalStats &Z(); - - /// \brief Get mutable reference to statistics for magnitude of signal. - /// \return Statistics for magnitude of signal. - public: SignalStats &Mag(); - - /// \brief Pointer to private data. - protected: Vector3StatsPrivate *dataPtr; - }; - } - } -} -#endif - diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh index 9d81d1d85..a41a0d2d4 100644 --- a/include/ignition/math/Vector4.hh +++ b/include/ignition/math/Vector4.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Open Source Robotics Foundation + * 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. @@ -13,743 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_VECTOR4_HH_ -#define IGNITION_MATH_VECTOR4_HH_ + */ -#include -#include -#include - -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - /// \class Vector4 Vector4.hh ignition/math/Vector4.hh - /// \brief T Generic x, y, z, w vector - template - class Vector4 - { - /// \brief math::Vector4(0, 0, 0, 0) - public: static const Vector4 Zero; - - /// \brief math::Vector4(1, 1, 1, 1) - public: static const Vector4 One; - - /// \brief math::Vector4(NaN, NaN, NaN, NaN) - public: static const Vector4 NaN; - - /// \brief Constructor - public: Vector4() - { - this->data[0] = this->data[1] = this->data[2] = this->data[3] = 0; - } - - /// \brief Constructor with component values - /// \param[in] _x value along x axis - /// \param[in] _y value along y axis - /// \param[in] _z value along z axis - /// \param[in] _w value along w axis - public: Vector4(const T &_x, const T &_y, const T &_z, const T &_w) - { - this->data[0] = _x; - this->data[1] = _y; - this->data[2] = _z; - this->data[3] = _w; - } - - /// \brief Copy constructor - /// \param[in] _v vector - public: Vector4(const Vector4 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - this->data[2] = _v[2]; - this->data[3] = _v[3]; - } - - /// \brief Destructor - public: virtual ~Vector4() {} - - /// \brief Calc distance to the given point - /// \param[in] _pt the point - /// \return the distance - public: T Distance(const Vector4 &_pt) const - { - return static_cast(sqrt( - (this->data[0]-_pt[0])*(this->data[0]-_pt[0]) + - (this->data[1]-_pt[1])*(this->data[1]-_pt[1]) + - (this->data[2]-_pt[2])*(this->data[2]-_pt[2]) + - (this->data[3]-_pt[3])*(this->data[3]-_pt[3]))); - } - - /// \brief Calc distance to the given point - /// \param[in] _x value along x - /// \param[in] _y value along y - /// \param[in] _z value along z - /// \param[in] _w value along w - /// \return the distance - public: T Distance(T _x, T _y, T _z, T _w) const - { - return this->Distance(Vector4(_x, _y, _z, _w)); - } - - /// \brief Returns the length (magnitude) of the vector - /// \return The length - public: T Length() const - { - return static_cast(sqrt(this->SquaredLength())); - } - - /// \brief Return the square of the length (magnitude) of the vector - /// \return the length - public: T SquaredLength() const - { - return - this->data[0] * this->data[0] + - this->data[1] * this->data[1] + - this->data[2] * this->data[2] + - this->data[3] * this->data[3]; - } - - /// \brief Round to near whole number. - public: void Round() - { - this->data[0] = static_cast(std::nearbyint(this->data[0])); - this->data[1] = static_cast(std::nearbyint(this->data[1])); - this->data[2] = static_cast(std::nearbyint(this->data[2])); - this->data[3] = static_cast(std::nearbyint(this->data[3])); - } - - /// \brief Get a rounded version of this vector - /// \return a rounded vector - public: Vector4 Rounded() const - { - Vector4 result = *this; - result.Round(); - return result; - } - - /// \brief Corrects any nan values - public: inline void Correct() - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - if (!std::isfinite(static_cast(this->data[0]))) - this->data[0] = 0; - if (!std::isfinite(static_cast(this->data[1]))) - this->data[1] = 0; - if (!std::isfinite(static_cast(this->data[2]))) - this->data[2] = 0; - if (!std::isfinite(static_cast(this->data[3]))) - this->data[3] = 0; - } - - /// \brief Normalize the vector length - public: void Normalize() - { - T d = this->Length(); - - if (!equal(d, static_cast(0.0))) - { - this->data[0] /= d; - this->data[1] /= d; - this->data[2] /= d; - this->data[3] /= d; - } - } - - /// \brief Return a normalized vector - /// \return unit length vector - public: Vector4 Normalized() const - { - Vector4 result = *this; - result.Normalize(); - return result; - } - - /// \brief Return the dot product of this vector and another vector - /// \param[in] _v the vector - /// \return the dot product - public: T Dot(const Vector4 &_v) const - { - return this->data[0] * _v[0] + - this->data[1] * _v[1] + - this->data[2] * _v[2] + - this->data[3] * _v[3]; - } - - /// \brief Return the absolute dot product of this vector and - /// another vector. This is similar to the Dot function, except the - /// absolute value of each component of the vector is used. - /// - /// result = abs(x1 * x2) + abs(y1 * y2) + abs(z1 * z2) + abs(w1 * w2) - /// - /// \param[in] _v the vector - /// \return The absolute dot product - public: T AbsDot(const Vector4 &_v) const - { - return std::abs(this->data[0] * _v[0]) + - std::abs(this->data[1] * _v[1]) + - std::abs(this->data[2] * _v[2]) + - std::abs(this->data[3] * _v[3]); - } - - /// \brief Get the absolute value of the vector - /// \return a vector with positive elements - public: Vector4 Abs() const - { - return Vector4(std::abs(this->data[0]), - std::abs(this->data[1]), - std::abs(this->data[2]), - std::abs(this->data[3])); - } - - /// \brief Set the contents of the vector - /// \param[in] _x value along x axis - /// \param[in] _y value along y axis - /// \param[in] _z value along z axis - /// \param[in] _w value along w axis - public: void Set(T _x = 0, T _y = 0, T _z = 0, T _w = 0) - { - this->data[0] = _x; - this->data[1] = _y; - this->data[2] = _z; - this->data[3] = _w; - } - - /// \brief Set this vector's components to the maximum of itself and the - /// passed in vector - /// \param[in] _v the maximum clamping vector - public: void Max(const Vector4 &_v) - { - this->data[0] = std::max(_v[0], this->data[0]); - this->data[1] = std::max(_v[1], this->data[1]); - this->data[2] = std::max(_v[2], this->data[2]); - this->data[3] = std::max(_v[3], this->data[3]); - } - - /// \brief Set this vector's components to the minimum of itself and the - /// passed in vector - /// \param[in] _v the minimum clamping vector - public: void Min(const Vector4 &_v) - { - this->data[0] = std::min(_v[0], this->data[0]); - this->data[1] = std::min(_v[1], this->data[1]); - this->data[2] = std::min(_v[2], this->data[2]); - this->data[3] = std::min(_v[3], this->data[3]); - } - - /// \brief Get the maximum value in the vector - /// \return the maximum element - public: T Max() const - { - return *std::max_element(this->data, this->data+4); - } - - /// \brief Get the minimum value in the vector - /// \return the minimum element - public: T Min() const - { - return *std::min_element(this->data, this->data+4); - } - - /// \brief Return the sum of the values - /// \return the sum - public: T Sum() const - { - return this->data[0] + this->data[1] + this->data[2] + this->data[3]; - } - - /// \brief Assignment operator - /// \param[in] _v the vector - /// \return a reference to this vector - public: Vector4 &operator=(const Vector4 &_v) - { - this->data[0] = _v[0]; - this->data[1] = _v[1]; - this->data[2] = _v[2]; - this->data[3] = _v[3]; - - return *this; - } - - /// \brief Assignment operator - /// \param[in] _value - public: Vector4 &operator=(T _value) - { - this->data[0] = _value; - this->data[1] = _value; - this->data[2] = _value; - this->data[3] = _value; - - return *this; - } - - /// \brief Addition operator - /// \param[in] _v the vector to add - /// \result a sum vector - public: Vector4 operator+(const Vector4 &_v) const - { - return Vector4(this->data[0] + _v[0], - this->data[1] + _v[1], - this->data[2] + _v[2], - this->data[3] + _v[3]); - } - - /// \brief Addition operator - /// \param[in] _v the vector to add - /// \return this vector - public: const Vector4 &operator+=(const Vector4 &_v) - { - this->data[0] += _v[0]; - this->data[1] += _v[1]; - this->data[2] += _v[2]; - this->data[3] += _v[3]; - - return *this; - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \return sum vector - public: inline Vector4 operator+(const T _s) const - { - return Vector4(this->data[0] + _s, - this->data[1] + _s, - this->data[2] + _s, - this->data[3] + _s); - } - - /// \brief Addition operators - /// \param[in] _s the scalar addend - /// \param[in] _v input vector - /// \return sum vector - public: friend inline Vector4 operator+(const T _s, - const Vector4 &_v) - { - return _v + _s; - } - - /// \brief Addition assignment operator - /// \param[in] _s scalar addend - /// \return this - public: const Vector4 &operator+=(const T _s) - { - this->data[0] += _s; - this->data[1] += _s; - this->data[2] += _s; - this->data[3] += _s; - - return *this; - } - - /// \brief Negation operator - /// \return negative of this vector - public: inline Vector4 operator-() const - { - return Vector4(-this->data[0], -this->data[1], - -this->data[2], -this->data[3]); - } - - /// \brief Subtraction operator - /// \param[in] _v the vector to substract - /// \return a vector - public: Vector4 operator-(const Vector4 &_v) const - { - return Vector4(this->data[0] - _v[0], - this->data[1] - _v[1], - this->data[2] - _v[2], - this->data[3] - _v[3]); - } - - /// \brief Subtraction assigment operators - /// \param[in] _v the vector to substract - /// \return this vector - public: const Vector4 &operator-=(const Vector4 &_v) - { - this->data[0] -= _v[0]; - this->data[1] -= _v[1]; - this->data[2] -= _v[2]; - this->data[3] -= _v[3]; - - return *this; - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar subtrahend - /// \return difference vector - public: inline Vector4 operator-(const T _s) const - { - return Vector4(this->data[0] - _s, - this->data[1] - _s, - this->data[2] - _s, - this->data[3] - _s); - } - - /// \brief Subtraction operators - /// \param[in] _s the scalar minuend - /// \param[in] _v vector subtrahend - /// \return difference vector - public: friend inline Vector4 operator-(const T _s, - const Vector4 &_v) - { - return {_s - _v.X(), _s - _v.Y(), _s - _v.Z(), _s - _v.W()}; - } - - /// \brief Subtraction assignment operator - /// \param[in] _s scalar subtrahend - /// \return this - public: const Vector4 &operator-=(const T _s) - { - this->data[0] -= _s; - this->data[1] -= _s; - this->data[2] -= _s; - this->data[3] -= _s; - - return *this; - } - - /// \brief Division assignment operator - /// \remarks Performs element wise division, - /// which has limited use. - /// \param[in] _v the vector to perform element wise division with - /// \return a result vector - public: const Vector4 operator/(const Vector4 &_v) const - { - return Vector4(this->data[0] / _v[0], - this->data[1] / _v[1], - this->data[2] / _v[2], - this->data[3] / _v[3]); - } - - /// \brief Division assignment operator - /// \remarks Performs element wise division, - /// which has limited use. - /// \param[in] _v the vector to perform element wise division with - /// \return this - public: const Vector4 &operator/=(const Vector4 &_v) - { - this->data[0] /= _v[0]; - this->data[1] /= _v[1]; - this->data[2] /= _v[2]; - this->data[3] /= _v[3]; - - return *this; - } - - /// \brief Division assignment operator - /// \remarks Performs element wise division, - /// which has limited use. - /// \param[in] _v another vector - /// \return a result vector - public: const Vector4 operator/(T _v) const - { - return Vector4(this->data[0] / _v, this->data[1] / _v, - this->data[2] / _v, this->data[3] / _v); - } - - /// \brief Division operator - /// \param[in] _v scaling factor - /// \return a vector - public: const Vector4 &operator/=(T _v) - { - this->data[0] /= _v; - this->data[1] /= _v; - this->data[2] /= _v; - this->data[3] /= _v; - - return *this; - } - - /// \brief Multiplication operator. - /// \remarks Performs element wise multiplication, - /// which has limited use. - /// \param[in] _pt another vector - /// \return result vector - public: const Vector4 operator*(const Vector4 &_pt) const - { - return Vector4(this->data[0] * _pt[0], - this->data[1] * _pt[1], - this->data[2] * _pt[2], - this->data[3] * _pt[3]); - } - - /// \brief Matrix multiplication operator. - /// \param[in] _m matrix - /// \return the vector multiplied by _m - public: const Vector4 operator*(const Matrix4 &_m) const - { - return Vector4( - this->data[0]*_m(0, 0) + this->data[1]*_m(1, 0) + - this->data[2]*_m(2, 0) + this->data[3]*_m(3, 0), - this->data[0]*_m(0, 1) + this->data[1]*_m(1, 1) + - this->data[2]*_m(2, 1) + this->data[3]*_m(3, 1), - this->data[0]*_m(0, 2) + this->data[1]*_m(1, 2) + - this->data[2]*_m(2, 2) + this->data[3]*_m(3, 2), - this->data[0]*_m(0, 3) + this->data[1]*_m(1, 3) + - this->data[2]*_m(2, 3) + this->data[3]*_m(3, 3)); - } - - /// \brief Multiplication assignment operator - /// \remarks Performs element wise multiplication, - /// which has limited use. - /// \param[in] _pt a vector - /// \return this - public: const Vector4 &operator*=(const Vector4 &_pt) - { - this->data[0] *= _pt[0]; - this->data[1] *= _pt[1]; - this->data[2] *= _pt[2]; - this->data[3] *= _pt[3]; - - return *this; - } - - /// \brief Multiplication operators - /// \param[in] _v scaling factor - /// \return a scaled vector - public: const Vector4 operator*(T _v) const - { - return Vector4(this->data[0] * _v, this->data[1] * _v, - this->data[2] * _v, this->data[3] * _v); - } - - /// \brief Scalar left multiplication operators - /// \param[in] _s the scaling factor - /// \param[in] _v the vector to scale - /// \return a scaled vector - public: friend inline const Vector4 operator*(const T _s, - const Vector4 &_v) - { - return Vector4(_v * _s); - } - - /// \brief Multiplication assignment operator - /// \param[in] _v scaling factor - /// \return this - public: const Vector4 &operator*=(T _v) - { - this->data[0] *= _v; - this->data[1] *= _v; - this->data[2] *= _v; - this->data[3] *= _v; - - return *this; - } - - /// \brief Equality test with tolerance. - /// \param[in] _v the vector to compare to - /// \param[in] _tol equality tolerance. - /// \return true if the elements of the vectors are equal within - /// the tolerence specified by _tol. - public: bool Equal(const Vector4 &_v, const T &_tol) const - { - return equal(this->data[0], _v[0], _tol) - && equal(this->data[1], _v[1], _tol) - && equal(this->data[2], _v[2], _tol) - && equal(this->data[3], _v[3], _tol); - } - - /// \brief Equal to operator - /// \param[in] _v the other vector - /// \return true if each component is equal within a - /// default tolerence (1e-6), false otherwise - public: bool operator==(const Vector4 &_v) const - { - return this->Equal(_v, static_cast(1e-6)); - } - - /// \brief Not equal to operator - /// \param[in] _pt the other vector - /// \return false if each component is equal within a - /// default tolerence (1e-6), true otherwise - public: bool operator!=(const Vector4 &_pt) const - { - return !(*this == _pt); - } - - /// \brief See if a point is finite (e.g., not nan) - /// \return true if finite, false otherwise - public: bool IsFinite() const - { - // std::isfinite works with floating point values, - // need to explicit cast to avoid ambiguity in vc++. - return std::isfinite(static_cast(this->data[0])) && - std::isfinite(static_cast(this->data[1])) && - std::isfinite(static_cast(this->data[2])) && - std::isfinite(static_cast(this->data[3])); - } - - /// \brief Array subscript operator - /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w. - /// The index is clamped to the range (0,3). - /// \return The value. - public: T &operator[](const std::size_t _index) - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; - } - - /// \brief Const-qualified array subscript operator - /// \param[in] _index The index, where 0 == x, 1 == y, 2 == z, 3 == w. - /// The index is clamped to the range (0,3). - /// \return The value. - public: T operator[](const std::size_t _index) const - { - return this->data[clamp(_index, IGN_ZERO_SIZE_T, IGN_THREE_SIZE_T)]; - } - - /// \brief Return a mutable x value. - /// \return The x component of the vector - public: T &X() - { - return this->data[0]; - } - - /// \brief Return a mutable y value. - /// \return The y component of the vector - public: T &Y() - { - return this->data[1]; - } - - /// \brief Return a mutable z value. - /// \return The z component of the vector - public: T &Z() - { - return this->data[2]; - } - - /// \brief Return a mutable w value. - /// \return The w component of the vector - public: T &W() - { - return this->data[3]; - } - - /// \brief Get the x value. - /// \return The x component of the vector - public: T X() const - { - return this->data[0]; - } - - /// \brief Get the y value. - /// \return The y component of the vector - public: T Y() const - { - return this->data[1]; - } - - /// \brief Get the z value. - /// \return The z component of the vector - public: T Z() const - { - return this->data[2]; - } - - /// \brief Get the w value. - /// \return The w component of the vector - public: T W() const - { - return this->data[3]; - } - - /// \brief Set the x value. - /// \param[in] _v Value for the x component. - public: inline void X(const T &_v) - { - this->data[0] = _v; - } - - /// \brief Set the y value. - /// \param[in] _v Value for the y component. - public: inline void Y(const T &_v) - { - this->data[1] = _v; - } - - /// \brief Set the z value. - /// \param[in] _v Value for the z component. - public: inline void Z(const T &_v) - { - this->data[2] = _v; - } - - /// \brief Set the w value. - /// \param[in] _v Value for the w component. - public: inline void W(const T &_v) - { - this->data[3] = _v; - } - - /// \brief Less than operator. - /// \param[in] _pt Vector to compare. - /// \return True if this vector's X(), Y(), Z() or W() value is less - /// than the given vector's corresponding values. - public: bool operator<(const Vector4 &_pt) const - { - return this->data[0] < _pt[0] || this->data[1] < _pt[1] || - this->data[2] < _pt[2] || this->data[3] < _pt[3]; - } - - /// \brief Stream insertion operator - /// \param[in] _out output stream - /// \param[in] _pt Vector4 to output - /// \return The stream - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Vector4 &_pt) - { - _out << _pt[0] << " " << _pt[1] << " " << _pt[2] << " " << _pt[3]; - return _out; - } - - /// \brief Stream extraction operator - /// \param[in] _in input stream - /// \param[in] _pt Vector4 to read values into - /// \return the stream - public: friend std::istream &operator>>( - std::istream &_in, ignition::math::Vector4 &_pt) - { - T x, y, z, w; - - // Skip white spaces - _in.setf(std::ios_base::skipws); - _in >> x >> y >> z >> w; - if (!_in.fail()) - { - _pt.Set(x, y, z, w); - } - return _in; - } - - /// \brief Data values, 0==x, 1==y, 2==z, 3==w - private: T data[4]; - }; - - template - const Vector4 Vector4::Zero(0, 0, 0, 0); - - template - const Vector4 Vector4::One(1, 1, 1, 1); - - template const Vector4 Vector4::NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - - typedef Vector4 Vector4i; - typedef Vector4 Vector4d; - typedef Vector4 Vector4f; - } - } -} -#endif diff --git a/include/ignition/math/config.hh b/include/ignition/math/config.hh new file mode 100644 index 000000000..716d3266e --- /dev/null +++ b/include/ignition/math/config.hh @@ -0,0 +1,39 @@ +/* + * 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 IGNITION_MATH__CONFIG_HH_ +#define IGNITION_MATH__CONFIG_HH_ + +#include + +/* Version number */ +// #define IGNITION_MATH_MAJOR_VERSION GZ_MATH_MAJOR_VERSION +// #define IGNITION_MATH_MINOR_VERSION GZ_MATH_MINOR_VERSION +// #define IGNITION_MATH_PATCH_VERSION GZ_MATH_PATCH_VERSION + +// #define IGNITION_MATH_VERSION GZ_MATH_VERSION +// #define IGNITION_MATH_VERSION_FULL GZ_MATH_VERSION_FULL + +// #define IGNITION_MATH_VERSION_NAMESPACE GZ_MATH_VERSION_NAMESPACE + +// #define IGNITION_MATH_VERSION_HEADER GZ_MATH_VERSION_HEADER + +/* #undef IGNITION_MATH_BUILD_TYPE_PROFILE */ +/* #undef IGNITION_MATH_BUILD_TYPE_DEBUG */ +/* #undef IGNITION_MATH_BUILD_TYPE_RELEASE */ + +#endif diff --git a/include/ignition/math/graph/Edge.hh b/include/ignition/math/graph/Edge.hh index db68db461..9a36fadd7 100644 --- a/include/ignition/math/graph/Edge.hh +++ b/include/ignition/math/graph/Edge.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,329 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_GRAPH_EDGE_HH_ -#define IGNITION_MATH_GRAPH_EDGE_HH_ - -// uint64_t -#include -#include -#include -#include -#include + */ +#include #include -#include "ignition/math/graph/Vertex.hh" - -namespace ignition -{ -namespace math -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_MATH_VERSION_NAMESPACE { -namespace graph -{ - /// \typedef EdgeId. - /// \brief The unique Id for an edge. - using EdgeId = uint64_t; - - /// \brief Used in the Graph constructors for uniform initialization. - template - struct EdgeInitializer - { - /// \brief Constructor. - /// \param[in] _vertices The vertices of the edge. - /// \param[in] _data The data stored in the edge. - /// \param[in] _weight The weight (cost) of the edge. - EdgeInitializer(const VertexId_P &_vertices, - const E &_data = E(), - const double _weight = 1) - : vertices(_vertices), - data(_data), - weight(_weight) - { - }; - - /// \brief IDs of the vertices. - public: VertexId_P vertices; - - /// \brief User data. - public: E data; - - /// \brief The weight (cost) of the edge. - public: double weight = 1; - }; - - /// \brief Generic edge class. An edge has two ends and some constraint - /// between them. For example, a directed edge only allows traversing the - /// edge in one direction. - template - class Edge - { - /// \brief Constructor. - /// \param[in] _vertices The vertices of the edge. - /// \param[in] _data The data stored in the edge. - /// \param[in] _weight The weight (cost) of the edge. - /// \param[in] _id Optional unique id. - public: explicit Edge(const VertexId_P &_vertices, - const E &_data, - const double _weight, - const EdgeId &_id = kNullId) - : id(_id), - vertices(_vertices), - data(_data), - weight(_weight) - { - } - - /// \brief Get the edge Id. - /// \return The edge Id. - public: EdgeId Id() const - { - return this->id; - } - - /// \brief Get the two vertices contained in the edge. - /// \return The two vertices contained in the edge. - public: VertexId_P Vertices() const - { - if (!this->Valid()) - return {kNullId, kNullId}; - - return this->vertices; - } - - /// \brief Get a non-mutable reference to the user data stored in the edge - /// \return The non-mutable reference to the user data stored in the edge. - public: const E &Data() const - { - return this->data; - } - - /// \brief Get a mutable reference to the user data stored in the edge. - /// \return The mutable reference to the user data stored in the edge. - public: E &Data() - { - return this->data; - } - - /// \brief The cost of traversing the _from end to the other end of the - /// edge. - /// \return The cost. - public: double Weight() const - { - return this->weight; - } - - /// \brief Set the cost of the edge. - /// \param[in] _newWeight The new cost. - public: void SetWeight(const double _newWeight) - { - this->weight = _newWeight; - } - - /// \brief Get the destination end that is reachable from a source end of - /// an edge. - /// - /// E.g.: Let's assume that we have an undirected edge (e1) with ends - /// (v1) and (v2): (v1)--(v2). The operation e1.From(v1) returns (v2). - /// The operation e1.From(v2) returns (v1). - /// - /// E.g.: Let's assume that we have a directed edge (e2) with the tail end - /// (v1) and the head end (v2): (v1)->(v2). The operation e2.From(v1) - /// returns (v2). The operation e2.From(v2) returns kNullId. - /// - /// \param[in] _from Source vertex. - /// \return The other vertex of the edge reachable from the "_from" - /// vertex or kNullId otherwise. - public: virtual VertexId From(const VertexId &_from) const = 0; - - /// \brief Get the source end that can reach the destination end of - /// an edge. - /// - /// E.g.: Let's assume that we have an undirected edge (e1) with ends - /// (v1) and (v2): (v1)--(v2). The operation e1.To(v1) returns (v2). - /// The operation e1.To(v2) returns (v1). - /// - /// E.g.: Let's assume that we have a directed edge (e2) with the tail end - /// (v1) and the head end (v2): (v1)->(v2). The operation e2.To(v1) - /// returns kNullId. The operation e2.To(v2) returns v1. - /// - /// \param[in] _to Destination vertex. - /// \return The other vertex of the edge that can reach "_to" - /// vertex or kNullId otherwise. - public: virtual VertexId To(const VertexId &_to) const = 0; - - /// \brief An edge is considered valid when its id is not kNullId. - /// \return Whether the edge is valid or not. - public: bool Valid() const - { - return this->id != kNullId; - } - - /// \brief Unique edge Id. - private: EdgeId id = kNullId; - - /// \brief The set of Ids of the two vertices. - private: VertexId_P vertices; - - /// \brief User data. - private: E data; - - /// \brief The weight (cost) of the edge. By default, the cost of an edge - /// is 1.0 . - private: double weight = 1.0; - }; - - /// \def EdgeId_S - /// \brief A set of edge Ids. - using EdgeId_S = std::set; - - /// \def EdgeRef_M - /// \brief A map of edges. The key is the edge Id. The value is a reference - /// to the edge. - template - using EdgeRef_M = std::map>; - - /// \brief An undirected edge represents a connection between two vertices. - /// The connection is bidirectional, it's possible to traverse the edge - /// in both directions. - template - class UndirectedEdge : public Edge - { - /// \brief An invalid undirected edge. - public: static UndirectedEdge NullEdge; - - /// \brief Constructor. - /// \param[in] _vertices The vertices of the edge. - /// \param[in] _data The data stored in the edge. - /// \param[in] _weight The weight (cost) of the edge. - /// \param[in] _id Optional unique id. - public: explicit UndirectedEdge(const VertexId_P &_vertices, - const E &_data, - const double _weight, - const EdgeId &_id = kNullId) - : Edge(_vertices, _data, _weight, _id) - { - } - - // Documentation inherited. - public: VertexId From(const VertexId &_from) const override - { - if (!this->Valid()) - return kNullId; - - if (this->Vertices().first != _from && this->Vertices().second != _from) - return kNullId; - - if (this->Vertices().first == _from) - return this->Vertices().second; - - return this->Vertices().first; - } - - // Documentation inherited. - public: VertexId To(const VertexId &_to) const override - { - return this->From(_to); - } - - /// \brief Stream insertion operator. The output uses DOT graph - /// description language. - /// \param[out] _out The output stream. - /// \param[in] _e Edge to write to the stream. - /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). - public: friend std::ostream &operator<<(std::ostream &_out, - const UndirectedEdge &_e) - { - auto vertices = _e.Vertices(); - _out << " " << vertices.first << " -- " << vertices.second - << " [label=" << _e.Weight() << "];" << std::endl; - return _out; - } - }; - - /// \brief An invalid undirected edge. - template - UndirectedEdge UndirectedEdge::NullEdge( - VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); - - /// \brief A directed edge represents a connection between two vertices. - /// The connection is unidirectional, it's only possible to traverse the edge - /// in one direction (from the tail to the head). - template - class DirectedEdge : public Edge - { - /// \brief An invalid directed edge. - public: static DirectedEdge NullEdge; - - /// \brief Constructor. - /// \param[in] _vertices The vertices of the edge. - /// \param[in] _data The data stored in the edge. - /// \param[in] _weight The weight (cost) of the edge. - /// \param[in] _id Optional unique id. - public: explicit DirectedEdge(const VertexId_P &_vertices, - const E &_data, - const double _weight, - const EdgeId &_id = kNullId) - : Edge(_vertices, _data, _weight, _id) - { - } - - /// \brief Get the Id of the tail vertex in this edge. - /// \return An id of the tail vertex in this edge. - /// \sa Head() - public: VertexId Tail() const - { - return this->Vertices().first; - } - - /// \brief Get the Id of the head vertex in this edge. - /// \return An id of the head vertex in this edge. - /// \sa Tail() - public: VertexId Head() const - { - return this->Vertices().second; - } - - // Documentation inherited. - public: VertexId From(const VertexId &_from) const override - { - if (_from != this->Tail()) - return kNullId; - - return this->Head(); - } - - // Documentation inherited. - public: VertexId To(const VertexId &_to) const override - { - if (_to != this->Head()) - return kNullId; - - return this->Tail(); - } - - /// \brief Stream insertion operator. The output uses DOT graph - /// description language. - /// \param[out] _out The output stream. - /// \param[in] _e Edge to write to the stream. - /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). - public: friend std::ostream &operator<<(std::ostream &_out, - const DirectedEdge &_e) - { - _out << " " << _e.Tail() << " -> " << _e.Head() - << " [label=" << _e.Weight() << "];" << std::endl; - return _out; - } - }; - - /// \brief An invalid directed edge. - template - DirectedEdge DirectedEdge::NullEdge( - VertexId_P(kNullId, kNullId), E(), 1.0, kNullId); -} -} -} -} -#endif diff --git a/include/ignition/math/graph/Graph.hh b/include/ignition/math/graph/Graph.hh index 02e402371..cae7123d7 100644 --- a/include/ignition/math/graph/Graph.hh +++ b/include/ignition/math/graph/Graph.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,797 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_GRAPH_GRAPH_HH_ -#define IGNITION_MATH_GRAPH_GRAPH_HH_ - -#include -#include -#include -#include -#include -#include -#include + */ +#include #include -#include "ignition/math/graph/Edge.hh" -#include "ignition/math/graph/Vertex.hh" - -namespace ignition -{ -namespace math -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_MATH_VERSION_NAMESPACE { -namespace graph -{ - /// \brief A generic graph class. - /// Both vertices and edges can store user information. A vertex could be - /// created passing a custom Id if needed, otherwise it will be choosen - /// internally. The vertices also have a name that could be reused among - /// other vertices if needed. This class supports the use of different edge - /// types (e.g. directed or undirected edges). - /// - /// Example directed graph - // - /// \code{.cpp} - /// - /// // Create a directed graph that is capable of storing integer data in the - /// // vertices and double data on the edges. - /// ignition::math::graph::DirectedGraph graph( - /// // Create the vertices, with default data and vertex ids. - /// { - /// {"vertex1"}, {"vertex2"}, {"vertex3"} - /// }, - /// // Create the edges, with default data and weight values. - /// { - /// // Edge from vertex 0 to vertex 1. Each number refers to a vertex id. - /// // Vertex ids start from zero. - /// {{0, 1}}, {{1, 2}} - /// }); - /// - /// // You can assign data to vertices. - /// ignition::math::graph::DirectedGraph graph2( - /// // Create the vertices, with custom data and default vertex ids. - /// { - /// {"vertex1", 1}, {"vertex2", 2}, {"vertex3", 10} - /// }, - /// // Create the edges, with default data and weight values. - /// { - /// // Edge from vertex 0 to vertex 1. Each number refers to a vertex id - /// // specified above. - /// {{0, 2}}, {{1, 2}} - /// }); - /// - /// - /// // It's also possible to specify vertex ids. - /// ignition::math::graph::DirectedGraph graph3( - /// // Create the vertices with custom data and vertex ids. - /// { - /// {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} - /// }, - /// // Create the edges, with custom data and default weight values. - /// { - /// {{2, 3}, 6.3}, {{3, 4}, 4.2} - /// }); - /// - /// // Finally, you can also assign weights to the edges. - /// ignition::math::graph::DirectedGraph graph4( - /// // Create the vertices with custom data and vertex ids. - /// { - /// {"vertex1", 1, 2}, {"vertex2", 2, 3}, {"vertex3", 10, 4} - /// }, - /// // Create the edges, with custom data and default weight values - /// { - /// {{2, 3}, 6.3, 1.1}, {{3, 4}, 4.2, 2.3} - /// }); - /// \endcode - template - class Graph - { - /// \brief Default constructor. - public: Graph() = default; - - /// \brief Constructor. - /// \param[in] _vertices Collection of vertices. - /// \param[in] _edges Collection of edges. - public: Graph(const std::vector> &_vertices, - const std::vector> &_edges) - { - // Add all vertices. - for (auto const &v : _vertices) - { - if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid()) - { - std::cerr << "Invalid vertex with Id [" << v.Id() << "]. Ignoring." - << std::endl; - } - } - - // Add all edges. - for (auto const &e : _edges) - { - if (!this->AddEdge(e.vertices, e.data, e.weight).Valid()) - std::cerr << "Ignoring edge" << std::endl; - } - } - - /// \brief Add a new vertex to the graph. - /// \param[in] _name Name of the vertex. It doesn't have to be unique. - /// \param[in] _data Data to be stored in the vertex. - /// \param[in] _id Optional Id to be used for this vertex. This Id must - /// be unique. - /// \return A reference to the new vertex. - public: Vertex &AddVertex(const std::string &_name, - const V &_data, - const VertexId &_id = kNullId) - { - auto id = _id; - - // The user didn't provide an Id, we generate it. - if (id == kNullId) - { - id = this->NextVertexId(); - - // No space for new Ids. - if (id == kNullId) - { - std::cerr << "[Graph::AddVertex()] The limit of vertices has been " - << "reached. Ignoring vertex." << std::endl; - return Vertex::NullVertex; - } - } - - // Create the vertex. - auto ret = this->vertices.insert( - std::make_pair(id, Vertex(_name, _data, id))); - - // The Id already exists. - if (!ret.second) - { - std::cerr << "[Graph::AddVertex()] Repeated vertex [" << id << "]" - << std::endl; - return Vertex::NullVertex; - } - - // Link the vertex with an empty list of edges. - this->adjList[id] = EdgeId_S(); - - // Update the map of names. - this->names.insert(std::make_pair(_name, id)); - - return ret.first->second; - } - - /// \brief The collection of all vertices in the graph. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. - public: const VertexRef_M Vertices() const - { - VertexRef_M res; - for (auto const &v : this->vertices) - res.emplace(std::make_pair(v.first, std::cref(v.second))); - - return res; - } - - /// \brief The collection of all vertices in the graph with name == _name. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. - public: const VertexRef_M Vertices(const std::string &_name) const - { - VertexRef_M res; - for (auto const &vertex : this->vertices) - { - if (vertex.second.Name() == _name) - res.emplace(std::make_pair(vertex.first, std::cref(vertex.second))); - } - - return res; - } - - /// \brief Add a new edge to the graph. - /// \param[in] _vertices The set of Ids of the two vertices. - /// \param[in] _data User data. - /// \param[in] _weight Edge weight. - /// \return Reference to the new edge created or NullEdge if the - /// edge was not created (e.g. incorrect vertices). - public: EdgeType &AddEdge(const VertexId_P &_vertices, - const E &_data, - const double _weight = 1.0) - { - auto id = this->NextEdgeId(); - - // No space for new Ids. - if (id == kNullId) - { - std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. " - << "Ignoring edge." << std::endl; - return EdgeType::NullEdge; - } - - EdgeType newEdge(_vertices, _data, _weight, id); - return this->LinkEdge(std::move(newEdge)); - } - - /// \brief Links an edge to the graph. This function verifies that the - /// edge's two vertices exist in the graph, copies the edge into the - /// graph's internal data structure, and returns a reference to this - /// new edge. - /// \param[in] _edge An edge to copy into the graph. - /// \return A reference to the new edge. - public: EdgeType &LinkEdge(const EdgeType &_edge) - { - auto edgeVertices = _edge.Vertices(); - - // Sanity check: Both vertices should exist. - for (auto const &v : {edgeVertices.first, edgeVertices.second}) - { - if (this->vertices.find(v) == this->vertices.end()) - return EdgeType::NullEdge; - } - - // Link the new edge. - for (auto const &v : {edgeVertices.first, edgeVertices.second}) - { - if (v != kNullId) - { - auto vertexIt = this->adjList.find(v); - assert(vertexIt != this->adjList.end()); - vertexIt->second.insert(_edge.Id()); - } - } - - auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge)); - - // Return the new edge. - return ret.first->second; - } - - /// \brief The collection of all edges in the graph. - /// \return A map of edges, where keys are Ids and values are references - /// to the edges. - public: const EdgeRef_M Edges() const - { - EdgeRef_M res; - for (auto const &edge : this->edges) - { - res.emplace(std::make_pair(edge.first, std::cref(edge.second))); - } - - return res; - } - - /// \brief Get all vertices that are directly connected with one edge - /// from a given vertex. In other words, this function will return - /// child vertices of the given vertex (all vertices from the given - /// vertex). E.g. j is adjacent from i (the given vertex) if there is an - /// edge (i->j). - /// - /// In an undirected graph, the result of this function will match - /// the result provided by AdjacentsTo. - /// - /// \param[in] _vertex The Id of the vertex from which adjacent - /// vertices will be returned. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. This is the set of adjacent vertices. - /// An empty map will be returned when the _vertex is not found in the - /// graph. - public: VertexRef_M AdjacentsFrom(const VertexId &_vertex) const - { - VertexRef_M res; - - // Make sure the vertex exists - auto vertexIt = this->adjList.find(_vertex); - if (vertexIt == this->adjList.end()) - return res; - - for (auto const &edgeId : vertexIt->second) - { - const auto &edge = this->EdgeFromId(edgeId); - auto neighborVertexId = edge.From(_vertex); - if (neighborVertexId != kNullId) - { - const auto &neighborVertex = this->VertexFromId(neighborVertexId); - res.emplace( - std::make_pair(neighborVertexId, std::cref(neighborVertex))); - } - } - - return res; - } - - /// \brief Get all vertices that are directly connected with one edge - /// from a given vertex. In other words, this function will return - /// child vertices of the given vertex (all vertices from the given - /// vertex). E.g. j is adjacent from i (the given vertex) if there is an - /// edge (i->j). - /// - /// In an undirected graph, the result of this function will match - /// the result provided by AdjacentsTo. - /// - /// \param[in] _vertex The Id of the vertex from which adjacent - /// vertices will be returned. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. This is the set of adjacent vertices. - /// An empty map will be returned when the _vertex is not found in the - /// graph. - public: VertexRef_M AdjacentsFrom(const Vertex &_vertex) const - { - return this->AdjacentsFrom(_vertex.Id()); - } - - /// \brief Get all vertices that are directly connected with one edge - /// to a given vertex. In other words, this function will return - /// child vertices of the given vertex (all vertices from the given - /// vertex). - /// - /// In an undirected graph, the result of this function will match - /// the result provided by AdjacentsFrom. - /// - /// E.g. i is adjacent to j (the given vertex) if there is an - /// edge (i->j). - /// \param[in] _vertex The Id of the vertex to check adjacentsTo. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. An empty map is returned if the - /// _vertex is not present in this graph, or when there are no - /// adjacent vertices. - public: VertexRef_M AdjacentsTo(const VertexId &_vertex) const - { - auto incidentEdges = this->IncidentsTo(_vertex); - - VertexRef_M res; - for (auto const &incidentEdgeRef : incidentEdges) - { - const auto &incidentEdgeId = incidentEdgeRef.first; - const auto &incidentEdge = this->EdgeFromId(incidentEdgeId); - const auto &neighborVertexId = incidentEdge.To(_vertex); - const auto &neighborVertex = this->VertexFromId(neighborVertexId); - res.emplace( - std::make_pair(neighborVertexId, std::cref(neighborVertex))); - } - - return res; - } - - /// \brief Get all vertices that are directly connected with one edge - /// to a given vertex. In other words, this function will return - /// child vertices of the given vertex (all vertices from the given - /// vertex). - /// - /// In an undirected graph, the result of this function will match - /// the result provided by AdjacentsFrom. - /// - /// E.g. i is adjacent to j (the given vertex) if there is an - /// edge (i->j). - /// \param[in] _vertex The vertex to check adjacentsTo. - /// \return A map of vertices, where keys are Ids and values are - /// references to the vertices. An empty map is returned if the - /// _vertex is not present in this graph, or when there are no - /// adjacent vertices. - public: VertexRef_M AdjacentsTo(const Vertex &_vertex) const - { - return this->AdjacentsTo(_vertex.Id()); - } - - /// \brief Get the number of edges incident to a vertex. - /// \param[in] _vertex The vertex Id. - /// \return The number of edges incidents to a vertex. - public: size_t InDegree(const VertexId &_vertex) const - { - return this->IncidentsTo(_vertex).size(); - } - - /// \brief Get the number of edges incident to a vertex. - /// \param[in] _vertex The vertex. - /// \return The number of edges incidents to a vertex. - public: size_t InDegree(const Vertex &_vertex) const - { - return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size(); - } - - /// \brief Get the number of edges incident from a vertex. - /// \param[in] _vertex The vertex Id. - /// \return The number of edges incidents from a vertex. - public: size_t OutDegree(const VertexId &_vertex) const - { - return this->IncidentsFrom(_vertex).size(); - } - - /// \brief Get the number of edges incident from a vertex. - /// \param[in] _vertex The vertex. - /// \return The number of edges incidents from a vertex. - public: size_t OutDegree(const Vertex &_vertex) const - { - return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size(); - } - - /// \brief Get the set of outgoing edges from a given vertex. - /// \param[in] _vertex Id of the vertex. - /// \return A map of edges, where keys are Ids and values are - /// references to the edges. An empty map is returned when the provided - /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) - const - { - EdgeRef_M res; - - const auto &adjIt = this->adjList.find(_vertex); - if (adjIt == this->adjList.end()) - return res; - - const auto &edgeIds = adjIt->second; - for (auto const &edgeId : edgeIds) - { - const auto &edge = this->EdgeFromId(edgeId); - if (edge.From(_vertex) != kNullId) - res.emplace(std::make_pair(edge.Id(), std::cref(edge))); - } - - return res; - } - - /// \brief Get the set of outgoing edges from a given vertex. - /// \param[in] _vertex The vertex. - /// \return A map of edges, where keys are Ids and values are - /// references to the edges. An empty map is returned when the provided - /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom( - const Vertex &_vertex) const - { - return this->IncidentsFrom(_vertex.Id()); - } - - /// \brief Get the set of incoming edges to a given vertex. - /// \param[in] _vertex Id of the vertex. - /// \return A map of edges, where keys are Ids and values are - /// references to the edges. An empty map is returned when the provided - /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo( - const VertexId &_vertex) const - { - EdgeRef_M res; - - const auto &adjIt = this->adjList.find(_vertex); - if (adjIt == this->adjList.end()) - return res; - - const auto &edgeIds = adjIt->second; - for (auto const &edgeId : edgeIds) - { - const auto &edge = this->EdgeFromId(edgeId); - if (edge.To(_vertex) != kNullId) - res.emplace(std::make_pair(edge.Id(), std::cref(edge))); - } - - return res; - } - - /// \brief Get the set of incoming edges to a given vertex. - /// \param[in] _vertex The vertex. - /// \return A map of edges, where keys are Ids and values are - /// references to the edges. An empty map is returned when the provided - /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) - const - { - return this->IncidentsTo(_vertex.Id()); - } - - /// \brief Get whether the graph is empty. - /// \return True when there are no vertices in the graph or - /// false otherwise. - public: bool Empty() const - { - return this->vertices.empty(); - } - - /// \brief Remove an existing vertex from the graph. - /// \param[in] _vertex Id of the vertex to be removed. - /// \return True when the vertex was removed or false otherwise. - public: bool RemoveVertex(const VertexId &_vertex) - { - auto vIt = this->vertices.find(_vertex); - if (vIt == this->vertices.end()) - return false; - - std::string name = vIt->second.Name(); - - // Remove incident edges. - auto incidents = this->IncidentsTo(_vertex); - for (auto edgePair : incidents) - this->RemoveEdge(edgePair.first); - - // Remove all outgoing edges. - incidents = this->IncidentsFrom(_vertex); - for (auto edgePair : incidents) - this->RemoveEdge(edgePair.first); - - // Remove the vertex (key) from the adjacency list. - this->adjList.erase(_vertex); - - // Remove the vertex. - this->vertices.erase(_vertex); - - // Get an iterator to all vertices sharing name. - auto iterPair = this->names.equal_range(name); - for (auto it = iterPair.first; it != iterPair.second; ++it) - { - if (it->second == _vertex) - { - this->names.erase(it); - break; - } - } - - return true; - } - - /// \brief Remove an existing vertex from the graph. - /// \param[in] _vertex The vertex to be removed. - /// \return True when the vertex was removed or false otherwise. - public: bool RemoveVertex(Vertex &_vertex) - { - return this->RemoveVertex(_vertex.Id()); - } - - /// \brief Remove all vertices with name == _name. - /// \param[in] _name Name of the vertices to be removed. - /// \return The number of vertices removed. - public: size_t RemoveVertices(const std::string &_name) - { - size_t numVertices = this->names.count(_name); - size_t result = 0; - for (size_t i = 0; i < numVertices; ++i) - { - auto iter = this->names.find(_name); - if (this->RemoveVertex(iter->second)) - ++result; - } - - return result; - } - - /// \brief Remove an existing edge from the graph. After the removal, it - /// won't be possible to reach any of the vertices from the edge, unless - /// there are other edges that connect the to vertices. - /// \param[in] _edge Id of the edge to be removed. - /// \return True when the edge was removed or false otherwise. - public: bool RemoveEdge(const EdgeId &_edge) - { - auto edgeIt = this->edges.find(_edge); - if (edgeIt == this->edges.end()) - return false; - - auto edgeVertices = edgeIt->second.Vertices(); - - // Unlink the edge. - for (auto const &v : {edgeVertices.first, edgeVertices.second}) - { - if (edgeIt->second.From(v) != kNullId) - { - auto vertex = this->adjList.find(v); - assert(vertex != this->adjList.end()); - vertex->second.erase(_edge); - } - } - - this->edges.erase(_edge); - - return true; - } - - /// \brief Remove an existing edge from the graph. After the removal, it - /// won't be possible to reach any of the vertices from the edge, unless - /// there are other edges that connect the to vertices. - /// \param[in] _edge The edge to be removed. - /// \return True when the edge was removed or false otherwise. - public: bool RemoveEdge(EdgeType &_edge) - { - return this->RemoveEdge(_edge.Id()); - } - - /// \brief Get a reference to a vertex using its Id. - /// \param[in] _id The Id of the vertex. - /// \return A reference to the vertex with Id = _id or NullVertex if - /// not found. - public: const Vertex &VertexFromId(const VertexId &_id) const - { - auto iter = this->vertices.find(_id); - if (iter == this->vertices.end()) - return Vertex::NullVertex; - - return iter->second; - } - - /// \brief Get a mutable reference to a vertex using its Id. - /// \param[in] _id The Id of the vertex. - /// \return A mutable reference to the vertex with Id = _id or NullVertex - /// if not found. - public: Vertex &VertexFromId(const VertexId &_id) - { - auto iter = this->vertices.find(_id); - if (iter == this->vertices.end()) - return Vertex::NullVertex; - - return iter->second; - } - - /// \brief Get a reference to an edge based on two vertices. A NullEdge - /// object reference is returned if an edge with the two vertices is not - /// found. If there are multiple edges that match the provided vertices, - /// then first is returned. - /// \param[in] _sourceId Source vertex Id. - /// \param[in] _destId Destination vertex Id. - /// \return A reference to the first edge found, or NullEdge if - /// not found. - public: const EdgeType &EdgeFromVertices( - const VertexId _sourceId, const VertexId _destId) const - { - // Get the adjacency iterator for the source vertex. - const typename std::map::const_iterator &adjIt = - this->adjList.find(_sourceId); - - // Quit early if there is no adjacency entry - if (adjIt == this->adjList.end()) - return EdgeType::NullEdge; - - // Loop over the edges in the source vertex's adjacency list - for (std::set::const_iterator edgIt = adjIt->second.begin(); - edgIt != adjIt->second.end(); ++edgIt) - { - // Get an iterator to the actual edge - const typename std::map::const_iterator edgeIter = - this->edges.find(*edgIt); - - // Check if the edge has the correct source and destination. - if (edgeIter != this->edges.end() && - edgeIter->second.From(_sourceId) == _destId) - { - assert(edgeIter->second.To(_destId) == _sourceId); - return edgeIter->second; - } - } - - return EdgeType::NullEdge; - } - - /// \brief Get a reference to an edge using its Id. - /// \param[in] _id The Id of the edge. - /// \return A reference to the edge with Id = _id or NullEdge if - /// not found. - public: const EdgeType &EdgeFromId(const EdgeId &_id) const - { - auto iter = this->edges.find(_id); - if (iter == this->edges.end()) - return EdgeType::NullEdge; - - return iter->second; - } - - /// \brief Stream insertion operator. The output uses DOT graph - /// description language. - /// \param[out] _out The output stream. - /// \param[in] _g Graph to write to the stream. - /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). - public: template - friend std::ostream &operator<<(std::ostream &_out, - const Graph &_g); - - /// \brief Get an available Id to be assigned to a new vertex. - /// \return The next available Id or kNullId if there aren't ids available. - private: VertexId &NextVertexId() - { - while (this->vertices.find(this->nextVertexId) != this->vertices.end() - && this->nextVertexId < MAX_UI64) - { - ++this->nextVertexId; - } - - return this->nextVertexId; - } - - /// \brief Get an available Id to be assigned to a new edge. - /// \return The next available Id or kNullId if there aren't ids available. - private: VertexId &NextEdgeId() - { - while (this->edges.find(this->nextEdgeId) != this->edges.end() && - this->nextEdgeId < MAX_UI64) - { - ++this->nextEdgeId; - } - - return this->nextEdgeId; - } - - /// \brief The next vertex Id to be assigned to a new vertex. - protected: VertexId nextVertexId = 0u; - - /// \brief The next edge Id to be assigned to a new edge. - protected: VertexId nextEdgeId = 0u; - - /// \brief The set of vertices. - private: std::map> vertices; - - /// \brief The set of edges. - private: std::map edges; - - /// \brief The adjacency list. - /// A map where the keys are vertex Ids. For each vertex (v) - /// with id (vId), the map value contains a set of edge Ids. Each of - /// the edges (e) with Id (eId) represents a connected path from (v) to - /// another vertex via (e). - private: std::map adjList; - - /// \brief Association between names and vertices curently used. - private: std::multimap names; - }; - - ///////////////////////////////////////////////// - /// Partial template specification for undirected edges. - template - std::ostream &operator<<(std::ostream &_out, - const Graph> &_g) - { - _out << "graph {" << std::endl; - - // All vertices with the name and Id as a "label" attribute. - for (auto const &vertexMap : _g.Vertices()) - { - auto vertex = vertexMap.second.get(); - _out << vertex; - } - - // All edges. - for (auto const &edgeMap : _g.Edges()) - { - auto edge = edgeMap.second.get(); - _out << edge; - } - - _out << "}" << std::endl; - - return _out; - } - - ///////////////////////////////////////////////// - /// Partial template specification for directed edges. - template - std::ostream &operator<<(std::ostream &_out, - const Graph> &_g) - { - _out << "digraph {" << std::endl; - - // All vertices with the name and Id as a "label" attribute. - for (auto const &vertexMap : _g.Vertices()) - { - auto vertex = vertexMap.second.get(); - _out << vertex; - } - - // All edges. - for (auto const &edgeMap : _g.Edges()) - { - auto edge = edgeMap.second.get(); - _out << edge; - } - - _out << "}" << std::endl; - - return _out; - } - - /// \def UndirectedGraph - /// \brief An undirected graph. - template - using UndirectedGraph = Graph>; - - /// \def DirectedGraph - /// \brief A directed graph. - template - using DirectedGraph = Graph>; -} -} -} -} -#endif diff --git a/include/ignition/math/graph/GraphAlgorithms.hh b/include/ignition/math/graph/GraphAlgorithms.hh index 0d3677384..beb36fea4 100644 --- a/include/ignition/math/graph/GraphAlgorithms.hh +++ b/include/ignition/math/graph/GraphAlgorithms.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,347 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_ -#define IGNITION_MATH_GRAPH_GRAPHALGORITHMS_HH_ - -#include -#include -#include -#include -#include -#include -#include + */ +#include #include -#include "ignition/math/graph/Graph.hh" -#include "ignition/math/Helpers.hh" - -namespace ignition -{ -namespace math -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_MATH_VERSION_NAMESPACE { -namespace graph -{ - /// \typedef CostInfo. - /// \brief Used in Dijkstra. For a given source vertex, this pair represents - /// the cost (first element) to reach a destination vertex (second element). - using CostInfo = std::pair; - - /// \brief Breadth first sort (BFS). - /// Starting from the vertex == _from, it traverses the graph exploring the - /// neighbors first, before moving to the next level neighbors. - /// \param[in] _graph A graph. - /// \param[in] _from The starting vertex. - /// \return The vector of vertices Ids traversed in a breadth first manner. - template - std::vector BreadthFirstSort(const Graph &_graph, - const VertexId &_from) - { - // Create an auxiliary graph, where the data is just a boolean value that - // stores whether the vertex has been visited or not. - Graph visitorGraph; - - // Copy the vertices (just the Id). - for (auto const &v : _graph.Vertices()) - visitorGraph.AddVertex("", false, v.first); - - // Copy the edges (without data). - for (auto const &e : _graph.Edges()) - visitorGraph.AddEdge(e.second.get().Vertices(), E()); - - std::vector visited; - std::list pending = {_from}; - - while (!pending.empty()) - { - auto vId = pending.front(); - pending.pop_front(); - - // If the vertex has been visited, skip. - auto &vertex = visitorGraph.VertexFromId(vId); - if (vertex.Data()) - continue; - - visited.push_back(vId); - vertex.Data() = true; - - // Add more vertices to visit if they haven't been visited yet. - auto adjacents = visitorGraph.AdjacentsFrom(vId); - for (auto const &adj : adjacents) - { - vId = adj.first; - auto &v = adj.second.get(); - if (!v.Data()) - pending.push_back(vId); - } - } - - return visited; - } - - /// \brief Depth first sort (DFS). - /// Starting from the vertex == _from, it visits the graph as far as - /// possible along each branch before backtracking. - /// \param[in] _graph A graph. - /// \param[in] _from The starting vertex. - /// \return The vector of vertices Ids visited in a depth first manner. - template - std::vector DepthFirstSort(const Graph &_graph, - const VertexId &_from) - { - // Create an auxiliary graph, where the data is just a boolean value that - // stores whether the vertex has been visited or not. - Graph visitorGraph; - - // Copy the vertices (just the Id). - for (auto const &v : _graph.Vertices()) - visitorGraph.AddVertex("", false, v.first); - - // Copy the edges (without data). - for (auto const &e : _graph.Edges()) - visitorGraph.AddEdge(e.second.get().Vertices(), E()); - - std::vector visited; - std::stack pending({_from}); - - while (!pending.empty()) - { - auto vId = pending.top(); - pending.pop(); - - // If the vertex has been visited, skip. - auto &vertex = visitorGraph.VertexFromId(vId); - if (vertex.Data()) - continue; - - visited.push_back(vId); - vertex.Data() = true; - - // Add more vertices to visit if they haven't been visited yet. - auto adjacents = visitorGraph.AdjacentsFrom(vId); - for (auto const &adj : adjacents) - { - vId = adj.first; - auto &v = adj.second.get(); - if (!v.Data()) - pending.push(vId); - } - } - - return visited; - } - - /// \brief Dijkstra algorithm. - /// Find the shortest path between the vertices in a graph. - /// If only a graph and a source vertex is provided, the algorithm will - /// find shortest paths from the source vertex to all other vertices in the - /// graph. If an additional destination vertex is provided, the algorithm - /// will stop when the shortest path is found between the source and - /// destination vertex. - /// \param[in] _graph A graph. - /// \param[in] _from The starting vertex. - /// \param[in] _to Optional destination vertex. - /// \return A map where the keys are the destination vertices. For each - /// destination, the value is another pair, where the key is the shortest - /// cost from the origin vertex. The value is the previous neighbor Id in the - /// shortest path. - /// Note: In the case of providing a destination vertex, only the entry in the - /// map with key = _to should be used. The rest of the map may contain - /// incomplete information. If you want all shortest paths to all other - /// vertices, please remove the destination vertex. - /// If the source or destination vertex don't exist, the function will return - /// an empty map. - /// - /// E.g.: Given the following undirected graph, g, with five vertices: - /// - /// (6) | - /// 0-------1 | - /// | /|\ | - /// | / | \(5) | - /// | (2)/ | \ | - /// | / | 2 | - /// (1)| / (2)| / | - /// | / | /(5) | - /// |/ |/ | - /// 3-------4 | - /// (1) | - /// - /// This is the resut of Dijkstra(g, 0): - /// - /// \code - /// ================================ - /// | Dst | Cost | Previous vertex | - /// ================================ - /// | 0 | 0 | 0 | - /// | 1 | 3 | 3 | - /// | 2 | 7 | 4 | - /// | 3 | 1 | 0 | - /// | 4 | 2 | 3 | - /// ================================ - /// \endcode - /// - /// This is the result of Dijkstra(g, 0, 3): - /// - /// \code - /// ================================ - /// | Dst | Cost | Previous vertex | - /// ================================ - /// | 0 | 0 | 0 | - /// | 1 |ignore| ignore | - /// | 2 |ignore| ignore | - /// | 3 | 1 | 0 | - /// | 4 |ignore| ignore | - /// ================================ - /// \endcode - /// - template - std::map Dijkstra(const Graph &_graph, - const VertexId &_from, - const VertexId &_to = kNullId) - { - auto allVertices = _graph.Vertices(); - - // Sanity check: The source vertex should exist. - if (allVertices.find(_from) == allVertices.end()) - { - std::cerr << "Vertex [" << _from << "] Not found" << std::endl; - return {}; - } - - // Sanity check: The destination vertex should exist (if used). - if (_to != kNullId && - allVertices.find(_to) == allVertices.end()) - { - std::cerr << "Vertex [" << _from << "] Not found" << std::endl; - return {}; - } - - // Store vertices that are being preprocessed. - std::priority_queue, std::greater> pq; - - // Create a map for distances and next neightbor and initialize all - // distances as infinite. - std::map dist; - for (auto const &v : allVertices) - { - auto id = v.first; - dist[id] = std::make_pair(MAX_D, kNullId); - } - - // Insert _from in the priority queue and initialize its distance as 0. - pq.push(std::make_pair(0.0, _from)); - dist[_from] = std::make_pair(0.0, _from); - - while (!pq.empty()) - { - // This is the minimum distance vertex. - VertexId u = pq.top().second; - - // Shortcut: Destination vertex found, exiting. - if (_to != kNullId && _to == u) - break; - - pq.pop(); - - for (auto const &edgePair : _graph.IncidentsFrom(u)) - { - const auto &edge = edgePair.second.get(); - const auto &v = edge.From(u); - double weight = edge.Weight(); - - // If there is shorted path to v through u. - if (dist[v].first > dist[u].first + weight) - { - // Updating distance of v. - dist[v] = std::make_pair(dist[u].first + weight, u); - pq.push(std::make_pair(dist[v].first, v)); - } - } - } - - return dist; - } - - /// \brief Calculate the connected components of an undirected graph. - /// A connected component of an undirected graph is a subgraph in which any - /// two vertices are connected to each other by paths, and which is connected - /// to no additional vertices in the supergraph. - /// \sa https://en.wikipedia.org/wiki/Connected_component_(graph_theory) - /// \param[in] _graph A graph. - /// \return A vector of graphs. Each element of the graph is a component - /// (subgraph) of the original graph. - template - std::vector> ConnectedComponents( - const UndirectedGraph &_graph) - { - std::map visited; - unsigned int componentCount = 0; - - for (auto const &v : _graph.Vertices()) - { - if (visited.find(v.first) == visited.end()) - { - auto component = BreadthFirstSort(_graph, v.first); - for (auto const &vId : component) - visited[vId] = componentCount; - ++componentCount; - } - } - - std::vector> res(componentCount); - - // Create the vertices. - for (auto const &vPair : _graph.Vertices()) - { - const auto &v = vPair.second.get(); - const auto &componentId = visited[v.Id()]; - res[componentId].AddVertex(v.Name(), v.Data(), v.Id()); - } - - // Create the edges. - for (auto const &ePair : _graph.Edges()) - { - const auto &e = ePair.second.get(); - const auto &vertices = e.Vertices(); - const auto &componentId = visited[vertices.first]; - res[componentId].AddEdge(vertices, e.Data(), e.Weight()); - } - - return res; - } - - /// \brief Copy a DirectedGraph to an UndirectedGraph with the same vertices - /// and edges. - /// \param[in] _graph A directed graph. - /// \return An undirected graph with the same vertices and edges as the - /// original graph. - template - UndirectedGraph ToUndirectedGraph(const DirectedGraph &_graph) - { - std::vector> vertices; - std::vector> edges; - - // Add all vertices. - for (auto const &vPair : _graph.Vertices()) - { - vertices.push_back(vPair.second.get()); - } - - // Add all edges. - for (auto const &ePair : _graph.Edges()) - { - auto const &e = ePair.second.get(); - edges.push_back({e.Vertices(), e.Data(), e.Weight()}); - } - - return UndirectedGraph(vertices, edges); - } -} -} -} -} -#endif diff --git a/include/ignition/math/graph/Vertex.hh b/include/ignition/math/graph/Vertex.hh index b65b73b6d..f1e2f7d45 100644 --- a/include/ignition/math/graph/Vertex.hh +++ b/include/ignition/math/graph/Vertex.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,139 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_GRAPH_VERTEX_HH_ -#define IGNITION_MATH_GRAPH_VERTEX_HH_ - -// uint64_t -#include -#include -#include -#include -#include -#include + */ +#include #include -#include - -namespace ignition -{ -namespace math -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_MATH_VERSION_NAMESPACE { -namespace graph -{ - /// \typedef VertexId. - /// \brief The unique Id of each vertex. - using VertexId = uint64_t; - - /// \def VertexId_P - /// \brief A pair of two vertex Ids. - using VertexId_P = std::pair; - - /// \brief Represents an invalid Id. - static const VertexId kNullId = MAX_UI64; - - /// \brief A vertex of a graph. It stores user information, an optional name, - /// and keeps an internal unique Id. This class does not enforce to choose a - /// unique name. - template - class Vertex - { - /// \brief An invalid vertex. - public: static Vertex NullVertex; - - /// \brief Constructor. - /// \param[in] _name Non-unique vertex name. - /// \param[in] _data User information. - /// \param[in] _id Optional unique id. - public: Vertex(const std::string &_name, - const V &_data = V(), - const VertexId _id = kNullId) - : name(_name), - data(_data), - id(_id) - { - } - - /// \brief Retrieve the user information. - /// \return Reference to the user information. - public: const V &Data() const - { - return this->data; - } - - /// \brief Get a mutable reference to the user information. - /// \return Mutable reference to the user information. - public: V &Data() - { - return this->data; - } - - /// \brief Get the vertex Id. - /// \return The vertex Id. - public: VertexId Id() const - { - return this->id; - } - - /// \brief Get the vertex name. - /// \return The vertex name. - public: const std::string &Name() const - { - return this->name; - } - - /// \brief Set the vertex name. - /// \param[in] _name The vertex name. - public: void SetName(const std::string &_name) - { - this->name = _name; - } - - /// \brief Whether the vertex is considered valid or not (id==kNullId). - /// \return True when the vertex is valid or false otherwise (id==kNullId) - public: bool Valid() const - { - return this->id != kNullId; - } - - /// \brief Stream insertion operator. The output uses DOT graph - /// description language. - /// \param[out] _out The output stream. - /// \param[in] _v Vertex to write to the stream. - /// \sa https://en.wikipedia.org/wiki/DOT_(graph_description_language). - public: friend std::ostream &operator<<(std::ostream &_out, - const Vertex &_v) - { - _out << " " << _v.Id() << " [label=\"" << _v.Name() - << " (" << _v.Id() << ")\"];" << std::endl; - return _out; - } - - /// \brief Non-unique vertex name. - private: std::string name = ""; - - /// \brief User information. - private: V data; - - /// \brief Unique vertex Id. - private: VertexId id = kNullId; - }; - - /// \brief An invalid vertex. - template - Vertex Vertex::NullVertex("__null__", V(), kNullId); - - /// \def VertexRef_M - /// \brief Map of vertices. The key is the vertex Id. The value is a - /// reference to the vertex. - template - using VertexRef_M = - std::map>>; -} -} -} -} -#endif diff --git a/src/AdditivelySeparableScalarField3_TEST.cc b/src/AdditivelySeparableScalarField3_TEST.cc index bbd52d323..78d4bd6ad 100644 --- a/src/AdditivelySeparableScalarField3_TEST.cc +++ b/src/AdditivelySeparableScalarField3_TEST.cc @@ -18,10 +18,10 @@ #include #include -#include "ignition/math/AdditivelySeparableScalarField3.hh" -#include "ignition/math/Polynomial3.hh" +#include "gz/math/AdditivelySeparableScalarField3.hh" +#include "gz/math/Polynomial3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(AdditivelySeparableScalarField3Test, Evaluate) diff --git a/src/Angle.cc b/src/Angle.cc index 296729a1c..632dee6c3 100644 --- a/src/Angle.cc +++ b/src/Angle.cc @@ -14,10 +14,10 @@ * limitations under the License. * */ -#include "ignition/math/Helpers.hh" -#include "ignition/math/Angle.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Angle.hh" -using namespace ignition::math; +using namespace gz::math; const Angle Angle::Zero = Angle(0); const Angle Angle::Pi = Angle(IGN_PI); diff --git a/src/Angle_TEST.cc b/src/Angle_TEST.cc index d99d7c7d0..8d4fe5f8f 100644 --- a/src/Angle_TEST.cc +++ b/src/Angle_TEST.cc @@ -19,10 +19,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Angle.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Angle.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(AngleTest, Angle) diff --git a/src/AxisAlignedBox.cc b/src/AxisAlignedBox.cc index af6f15a6d..2d308e825 100644 --- a/src/AxisAlignedBox.cc +++ b/src/AxisAlignedBox.cc @@ -15,13 +15,13 @@ * */ #include -#include +#include -using namespace ignition; +using namespace gz; using namespace math; // Private data for AxisAlignedBox class -class ignition::math::AxisAlignedBoxPrivate +class gz::math::AxisAlignedBoxPrivate { /// \brief Minimum corner of the box public: Vector3d min = Vector3d(MAX_D, MAX_D, MAX_D); diff --git a/src/AxisAlignedBox_TEST.cc b/src/AxisAlignedBox_TEST.cc index dadbf0388..79116cc94 100644 --- a/src/AxisAlignedBox_TEST.cc +++ b/src/AxisAlignedBox_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/AxisAlignedBox.hh" +#include "gz/math/AxisAlignedBox.hh" -using namespace ignition; +using namespace gz; using namespace math; class myAxisAlignedBox : public AxisAlignedBox diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 454abf4cb..8be010c6e 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Box.hh" +#include "gz/math/Box.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(BoxTest, Constructor) diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index bb09828d9..e2fdd0e8c 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -18,10 +18,10 @@ #include #include -#include "ignition/math/Capsule.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Capsule.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(CapsuleTest, Constructor) diff --git a/src/Color.cc b/src/Color.cc index 41dcd40a1..3fcbc24b9 100644 --- a/src/Color.cc +++ b/src/Color.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Color.hh" +#include "gz/math/Color.hh" -using namespace ignition; +using namespace gz; using namespace math; const Color Color::White = Color(1, 1, 1, 1); diff --git a/src/Color_TEST.cc b/src/Color_TEST.cc index 382d38673..b91ba1cb4 100644 --- a/src/Color_TEST.cc +++ b/src/Color_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include +#include -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Color, ConstColors) diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index c0ad31de2..c9e5df94e 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -18,9 +18,9 @@ #include #include -#include "ignition/math/Cylinder.hh" +#include "gz/math/Cylinder.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(CylinderTest, Constructor) diff --git a/src/DiffDriveOdometry.cc b/src/DiffDriveOdometry.cc index d184b8e02..f86905b57 100644 --- a/src/DiffDriveOdometry.cc +++ b/src/DiffDriveOdometry.cc @@ -15,15 +15,15 @@ * */ #include -#include "ignition/math/DiffDriveOdometry.hh" -#include "ignition/math/RollingMean.hh" +#include "gz/math/DiffDriveOdometry.hh" +#include "gz/math/RollingMean.hh" -using namespace ignition; +using namespace gz; using namespace math; // The implementation was borrowed from: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/diff_drive_controller/src/odometry.cpp -class ignition::math::DiffDriveOdometryPrivate +class gz::math::DiffDriveOdometryPrivate { /// \brief Integrates the velocities (linear and angular) using 2nd order /// Runge-Kutta. diff --git a/src/DiffDriveOdometry_TEST.cc b/src/DiffDriveOdometry_TEST.cc index 6bc531f11..68fd06c56 100644 --- a/src/DiffDriveOdometry_TEST.cc +++ b/src/DiffDriveOdometry_TEST.cc @@ -18,11 +18,11 @@ #include #include -#include "ignition/math/Angle.hh" -#include "ignition/math/Helpers.hh" -#include "ignition/math/DiffDriveOdometry.hh" +#include "gz/math/Angle.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/DiffDriveOdometry.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(DiffDriveOdometryTest, DiffDriveOdometry) diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 3d89f15df..951401bba 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -18,11 +18,11 @@ #include #include -#include "ignition/math/Ellipsoid.hh" -#include "ignition/math/Helpers.hh" -#include "ignition/math/Vector3.hh" +#include "gz/math/Ellipsoid.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Vector3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(EllipsoidTest, Constructor) diff --git a/src/Filter_TEST.cc b/src/Filter_TEST.cc index 84510665b..aa9e9264f 100644 --- a/src/Filter_TEST.cc +++ b/src/Filter_TEST.cc @@ -17,9 +17,9 @@ #include -#include "ignition/math/Filter.hh" +#include "gz/math/Filter.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(FilterTest, OnePole) diff --git a/src/Frustum.cc b/src/Frustum.cc index 22fa0ca6a..c835a3960 100644 --- a/src/Frustum.cc +++ b/src/Frustum.cc @@ -16,12 +16,12 @@ */ #include -#include "ignition/math/AxisAlignedBox.hh" -#include "ignition/math/Frustum.hh" -#include "ignition/math/Matrix4.hh" +#include "gz/math/AxisAlignedBox.hh" +#include "gz/math/Frustum.hh" +#include "gz/math/Matrix4.hh" #include "FrustumPrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/FrustumPrivate.hh b/src/FrustumPrivate.hh index 3077655e5..dae03a740 100644 --- a/src/FrustumPrivate.hh +++ b/src/FrustumPrivate.hh @@ -14,15 +14,15 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_FRUSTUMPRIVATE_HH_ -#define IGNITION_MATH_FRUSTUMPRIVATE_HH_ +#ifndef GZ_MATH_FRUSTUMPRIVATE_HH_ +#define GZ_MATH_FRUSTUMPRIVATE_HH_ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace ignition { diff --git a/src/Frustum_TEST.cc b/src/Frustum_TEST.cc index 492af9f32..0417eaa2a 100644 --- a/src/Frustum_TEST.cc +++ b/src/Frustum_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Frustum.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Frustum.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/GaussMarkovProcess.cc b/src/GaussMarkovProcess.cc index 311de7e54..118bbb3ed 100644 --- a/src/GaussMarkovProcess.cc +++ b/src/GaussMarkovProcess.cc @@ -15,13 +15,13 @@ * */ -#include -#include +#include +#include -using namespace ignition::math; +using namespace gz::math; ////////////////////////////////////////////////// -class ignition::math::GaussMarkovProcessPrivate +class gz::math::GaussMarkovProcessPrivate { /// \brief Current process value. public: double value{0}; diff --git a/src/GaussMarkovProcess_TEST.cc b/src/GaussMarkovProcess_TEST.cc index cc96cea56..cf239a083 100644 --- a/src/GaussMarkovProcess_TEST.cc +++ b/src/GaussMarkovProcess_TEST.cc @@ -17,11 +17,11 @@ #include -#include "ignition/math/GaussMarkovProcess.hh" -#include "ignition/math/Helpers.hh" -#include "ignition/math/Rand.hh" +#include "gz/math/GaussMarkovProcess.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Rand.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Helpers.cc b/src/Helpers.cc index 85c17962f..d4622b367 100644 --- a/src/Helpers.cc +++ b/src/Helpers.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include "ignition/math/Helpers.hh" +#include "gz/math/Helpers.hh" namespace ignition { diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc index a602f210d..7734d16ca 100644 --- a/src/Helpers_TEST.cc +++ b/src/Helpers_TEST.cc @@ -17,11 +17,11 @@ #include -#include "ignition/math/Rand.hh" -#include "ignition/math/Vector3.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Rand.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// // Test a few function in Helpers diff --git a/src/Inertial_TEST.cc b/src/Inertial_TEST.cc index b5793cd4f..55d804c16 100644 --- a/src/Inertial_TEST.cc +++ b/src/Inertial_TEST.cc @@ -18,9 +18,9 @@ #include #include -#include "ignition/math/Inertial.hh" +#include "gz/math/Inertial.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// /// \brief Compare quaternions, but allow rotations of PI about any axis. diff --git a/src/Interval_TEST.cc b/src/Interval_TEST.cc index ecd0c8ee1..f03332ea3 100644 --- a/src/Interval_TEST.cc +++ b/src/Interval_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Interval.hh" +#include "gz/math/Interval.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(IntervalTest, DefaultConstructor) diff --git a/src/Kmeans.cc b/src/Kmeans.cc index 41dce68c1..f8e2af4e9 100644 --- a/src/Kmeans.cc +++ b/src/Kmeans.cc @@ -15,14 +15,14 @@ * */ -#include +#include #include -#include +#include #include "KmeansPrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ////////////////////////////////////////////////// diff --git a/src/KmeansPrivate.hh b/src/KmeansPrivate.hh index bcda8ef77..8c868dccf 100644 --- a/src/KmeansPrivate.hh +++ b/src/KmeansPrivate.hh @@ -14,13 +14,13 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_KMEANSPRIVATE_HH_ -#define IGNITION_MATH_KMEANSPRIVATE_HH_ +#ifndef GZ_MATH_KMEANSPRIVATE_HH_ +#define GZ_MATH_KMEANSPRIVATE_HH_ #include -#include -#include -#include +#include +#include +#include namespace ignition { diff --git a/src/Kmeans_TEST.cc b/src/Kmeans_TEST.cc index 4478f268a..66e5bc996 100644 --- a/src/Kmeans_TEST.cc +++ b/src/Kmeans_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Kmeans.hh" +#include "gz/math/Kmeans.hh" -using namespace ignition; +using namespace gz; ////////////////////////////////////////////////// TEST(KmeansTest, Kmeans) diff --git a/src/Line2_TEST.cc b/src/Line2_TEST.cc index 6ac265446..d166d24c7 100644 --- a/src/Line2_TEST.cc +++ b/src/Line2_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Line2.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Line2.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Line2Test, Constructor) diff --git a/src/Line3_TEST.cc b/src/Line3_TEST.cc index db50be5b6..92c58a15f 100644 --- a/src/Line3_TEST.cc +++ b/src/Line3_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Line3.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Line3.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Line3Test, Constructor) diff --git a/src/MassMatrix3_TEST.cc b/src/MassMatrix3_TEST.cc index ec4a23d02..19cdff98f 100644 --- a/src/MassMatrix3_TEST.cc +++ b/src/MassMatrix3_TEST.cc @@ -18,11 +18,11 @@ #include #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(MassMatrix3dTest, Constructors) diff --git a/src/Material.cc b/src/Material.cc index 37893a5c8..ec2cc59d7 100644 --- a/src/Material.cc +++ b/src/Material.cc @@ -15,13 +15,13 @@ * */ -#include "ignition/math/Material.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Material.hh" +#include "gz/math/Helpers.hh" // Placing the kMaterialData in a separate file for conveniece and clarity. #include "MaterialType.hh" -using namespace ignition; +using namespace gz; using namespace math; // Initialize the static map of Material objects based on the kMaterialData. @@ -40,7 +40,7 @@ static const std::map kMaterials = []() }(); // Private data for the Material class -class ignition::math::MaterialPrivate +class gz::math::MaterialPrivate { /// \brief The material type. public: MaterialType type = MaterialType::UNKNOWN_MATERIAL; diff --git a/src/MaterialType.hh b/src/MaterialType.hh index cda91a23e..fc15e433a 100644 --- a/src/MaterialType.hh +++ b/src/MaterialType.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_MATERIAL_HH_ -#define IGNITION_MATERIAL_HH_ +#ifndef GZ_MATERIAL_HH_ +#define GZ_MATERIAL_HH_ #include #include diff --git a/src/Material_TEST.cc b/src/Material_TEST.cc index 1d79fc820..6c7a93731 100644 --- a/src/Material_TEST.cc +++ b/src/Material_TEST.cc @@ -16,11 +16,11 @@ */ #include -#include "ignition/math/Material.hh" -#include "ignition/math/MaterialType.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Material.hh" +#include "gz/math/MaterialType.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Matrix3_TEST.cc b/src/Matrix3_TEST.cc index 1cd0044cd..910bb6720 100644 --- a/src/Matrix3_TEST.cc +++ b/src/Matrix3_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Matrix3.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Matrix3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Matrix3dTest, Matrix3d) diff --git a/src/Matrix4_TEST.cc b/src/Matrix4_TEST.cc index d1459f87a..dc5e7ff35 100644 --- a/src/Matrix4_TEST.cc +++ b/src/Matrix4_TEST.cc @@ -17,12 +17,12 @@ #include -#include "ignition/math/Pose3.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Matrix4.hh" -#include "ignition/math/Vector3.hh" +#include "gz/math/Pose3.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Matrix4.hh" +#include "gz/math/Vector3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Matrix4dTest, Construct) diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index 27c9a56ac..8ec94a281 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -17,9 +17,9 @@ #include -#include "ignition/math/Matrix6.hh" +#include "gz/math/Matrix6.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/MovingWindowFilter_TEST.cc b/src/MovingWindowFilter_TEST.cc index f6189b202..a09ba5469 100644 --- a/src/MovingWindowFilter_TEST.cc +++ b/src/MovingWindowFilter_TEST.cc @@ -16,10 +16,10 @@ */ #include -#include "ignition/math/Vector3.hh" -#include "ignition/math/MovingWindowFilter.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/MovingWindowFilter.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(MovingWindowFilterTest, SetWindowSize) diff --git a/src/OrientedBox_TEST.cc b/src/OrientedBox_TEST.cc index 445d7c6b5..2265c3bf8 100644 --- a/src/OrientedBox_TEST.cc +++ b/src/OrientedBox_TEST.cc @@ -17,10 +17,10 @@ #include #include -#include "ignition/math/Angle.hh" -#include "ignition/math/OrientedBox.hh" +#include "gz/math/Angle.hh" +#include "gz/math/OrientedBox.hh" -using namespace ignition; +using namespace gz; using namespace math; auto g_tolerance = 1e-6; diff --git a/src/PID.cc b/src/PID.cc index 1ec3bd29c..c80989121 100644 --- a/src/PID.cc +++ b/src/PID.cc @@ -17,10 +17,10 @@ #include #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/PID.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/PID.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/PID_TEST.cc b/src/PID_TEST.cc index 1a0158316..ff58a25bf 100644 --- a/src/PID_TEST.cc +++ b/src/PID_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/PID.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/PID.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(PidTest, ConstructorDefault) diff --git a/src/PiecewiseScalarField3_TEST.cc b/src/PiecewiseScalarField3_TEST.cc index ac7c10c1f..40e63d769 100644 --- a/src/PiecewiseScalarField3_TEST.cc +++ b/src/PiecewiseScalarField3_TEST.cc @@ -19,12 +19,12 @@ #include #include -#include "ignition/math/AdditivelySeparableScalarField3.hh" -#include "ignition/math/PiecewiseScalarField3.hh" -#include "ignition/math/Polynomial3.hh" -#include "ignition/math/Vector3.hh" +#include "gz/math/AdditivelySeparableScalarField3.hh" +#include "gz/math/PiecewiseScalarField3.hh" +#include "gz/math/Polynomial3.hh" +#include "gz/math/Vector3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(PiecewiseScalarField3Test, Evaluate) diff --git a/src/Plane_TEST.cc b/src/Plane_TEST.cc index e082d2c26..a7970616d 100644 --- a/src/Plane_TEST.cc +++ b/src/Plane_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Plane.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Plane.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Polynomial3_TEST.cc b/src/Polynomial3_TEST.cc index dc5998cfa..e1b6f627d 100644 --- a/src/Polynomial3_TEST.cc +++ b/src/Polynomial3_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Polynomial3.hh" +#include "gz/math/Polynomial3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Polynomial3Test, DefaultConstructor) diff --git a/src/Pose_TEST.cc b/src/Pose_TEST.cc index 3313ba186..dde17ce9c 100644 --- a/src/Pose_TEST.cc +++ b/src/Pose_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Pose3.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Pose3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(PoseTest, Construction) diff --git a/src/Quaternion_TEST.cc b/src/Quaternion_TEST.cc index 5e4f0deb8..f7167163b 100644 --- a/src/Quaternion_TEST.cc +++ b/src/Quaternion_TEST.cc @@ -19,12 +19,12 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Matrix3.hh" -#include "ignition/math/Matrix4.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Matrix3.hh" +#include "gz/math/Matrix4.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(QuaternionTest, Construction) diff --git a/src/Rand.cc b/src/Rand.cc index 95fcae443..0cb9b7b9f 100644 --- a/src/Rand.cc +++ b/src/Rand.cc @@ -25,9 +25,9 @@ #include #endif -#include "ignition/math/Rand.hh" +#include "gz/math/Rand.hh" -using namespace ignition; +using namespace gz; using namespace math; ////////////////////////////////////////////////// diff --git a/src/Rand_TEST.cc b/src/Rand_TEST.cc index 447d2c7c3..df31afbdc 100644 --- a/src/Rand_TEST.cc +++ b/src/Rand_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Rand.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Rand.hh" -using namespace ignition; +using namespace gz; ////////////////////////////////////////////////// TEST(RandTest, Rand) diff --git a/src/Region3_TEST.cc b/src/Region3_TEST.cc index feb408919..2af3c1308 100644 --- a/src/Region3_TEST.cc +++ b/src/Region3_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include "ignition/math/Region3.hh" +#include "gz/math/Region3.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Region3Test, DefaultConstructor) diff --git a/src/RollingMean.cc b/src/RollingMean.cc index 3510d0619..7f7e21e74 100644 --- a/src/RollingMean.cc +++ b/src/RollingMean.cc @@ -18,12 +18,12 @@ #include #include #include -#include "ignition/math/RollingMean.hh" +#include "gz/math/RollingMean.hh" -using namespace ignition::math; +using namespace gz::math; /// \brief Private data -class ignition::math::RollingMeanPrivate +class gz::math::RollingMeanPrivate { /// \brief The window size public: size_t windowSize{10}; diff --git a/src/RollingMean_TEST.cc b/src/RollingMean_TEST.cc index 9ae85ffa4..4800003b8 100644 --- a/src/RollingMean_TEST.cc +++ b/src/RollingMean_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/RollingMean.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/RollingMean.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(RollingMeanTest, RollingMean) diff --git a/src/RotationSpline.cc b/src/RotationSpline.cc index 8fdc1fc0b..9f04abccf 100644 --- a/src/RotationSpline.cc +++ b/src/RotationSpline.cc @@ -14,11 +14,11 @@ * limitations under the License. * */ -#include "ignition/math/Quaternion.hh" -#include "ignition/math/RotationSpline.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/RotationSpline.hh" #include "RotationSplinePrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/RotationSplinePrivate.cc b/src/RotationSplinePrivate.cc index 1396e829e..e993c6830 100644 --- a/src/RotationSplinePrivate.cc +++ b/src/RotationSplinePrivate.cc @@ -16,7 +16,7 @@ */ #include "RotationSplinePrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/RotationSplinePrivate.hh b/src/RotationSplinePrivate.hh index 34af9b9f1..d980a6124 100644 --- a/src/RotationSplinePrivate.hh +++ b/src/RotationSplinePrivate.hh @@ -14,12 +14,12 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_ROTATIONSPLINEPRIVATE_HH_ -#define IGNITION_MATH_ROTATIONSPLINEPRIVATE_HH_ +#ifndef GZ_MATH_ROTATIONSPLINEPRIVATE_HH_ +#define GZ_MATH_ROTATIONSPLINEPRIVATE_HH_ #include -#include -#include "ignition/math/Quaternion.hh" +#include +#include "gz/math/Quaternion.hh" namespace ignition { diff --git a/src/RotationSpline_TEST.cc b/src/RotationSpline_TEST.cc index cf5b2816f..d01270b6c 100644 --- a/src/RotationSpline_TEST.cc +++ b/src/RotationSpline_TEST.cc @@ -17,12 +17,12 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Vector3.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/RotationSpline.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/RotationSpline.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(RotationSplineTest, RotationSpline) diff --git a/src/SemanticVersion.cc b/src/SemanticVersion.cc index ca594d588..a9c8c2fcd 100644 --- a/src/SemanticVersion.cc +++ b/src/SemanticVersion.cc @@ -17,9 +17,9 @@ #include -#include "ignition/math/SemanticVersion.hh" +#include "gz/math/SemanticVersion.hh" -using namespace ignition; +using namespace gz; using namespace math; namespace ignition diff --git a/src/SemanticVersion_TEST.cc b/src/SemanticVersion_TEST.cc index 66f60d268..6e3637ec5 100644 --- a/src/SemanticVersion_TEST.cc +++ b/src/SemanticVersion_TEST.cc @@ -17,9 +17,9 @@ #include -#include "ignition/math/SemanticVersion.hh" +#include "gz/math/SemanticVersion.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/SignalStats.cc b/src/SignalStats.cc index 4523bef3a..151c40d85 100644 --- a/src/SignalStats.cc +++ b/src/SignalStats.cc @@ -16,10 +16,10 @@ */ #include #include -#include +#include #include "SignalStatsPrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ////////////////////////////////////////////////// diff --git a/src/SignalStatsPrivate.hh b/src/SignalStatsPrivate.hh index e5e045e86..0d379cc16 100644 --- a/src/SignalStatsPrivate.hh +++ b/src/SignalStatsPrivate.hh @@ -14,12 +14,12 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_SIGNALSTATSPRIVATE_HH_ -#define IGNITION_MATH_SIGNALSTATSPRIVATE_HH_ +#ifndef GZ_MATH_SIGNALSTATSPRIVATE_HH_ +#define GZ_MATH_SIGNALSTATSPRIVATE_HH_ #include #include -#include +#include namespace ignition { diff --git a/src/SignalStats_TEST.cc b/src/SignalStats_TEST.cc index 8024fb93d..289deef78 100644 --- a/src/SignalStats_TEST.cc +++ b/src/SignalStats_TEST.cc @@ -17,10 +17,10 @@ #include -#include -#include +#include +#include -using namespace ignition; +using namespace gz; ////////////////////////////////////////////////// TEST(SignalStatsTest, SignalMaximumConstructor) diff --git a/src/SpeedLimiter.cc b/src/SpeedLimiter.cc index 02a0b8a99..fe2c1df6f 100644 --- a/src/SpeedLimiter.cc +++ b/src/SpeedLimiter.cc @@ -15,14 +15,14 @@ * */ -#include "ignition/math/Helpers.hh" -#include "ignition/math/SpeedLimiter.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/SpeedLimiter.hh" -using namespace ignition; +using namespace gz; using namespace math; /// \brief Private SpeedLimiter data class. -class ignition::math::SpeedLimiterPrivate +class gz::math::SpeedLimiterPrivate { /// \brief Minimum velocity limit. public: double minVelocity{-std::numeric_limits::infinity()}; diff --git a/src/SpeedLimiter_TEST.cc b/src/SpeedLimiter_TEST.cc index 7f4c46134..c6a2b6d44 100644 --- a/src/SpeedLimiter_TEST.cc +++ b/src/SpeedLimiter_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/SpeedLimiter.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/SpeedLimiter.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace std::literals::chrono_literals; diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index cebc5b657..0f99d569d 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -18,9 +18,9 @@ #include #include -#include "ignition/math/Sphere.hh" +#include "gz/math/Sphere.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index abbb313b2..915446695 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -16,10 +16,10 @@ */ #include -#include "ignition/math/Matrix3.hh" -#include "ignition/math/SphericalCoordinates.hh" +#include "gz/math/Matrix3.hh" +#include "gz/math/SphericalCoordinates.hh" -using namespace ignition; +using namespace gz; using namespace math; // Parameters for EARTH_WGS84 model @@ -38,7 +38,7 @@ const double g_EarthWGS84Flattening = 1.0/298.257223563; const double g_EarthRadius = 6371000.0; // Private data for the SphericalCoordinates class. -class ignition::math::SphericalCoordinatesPrivate +class gz::math::SphericalCoordinatesPrivate { /// \brief Type of surface being used. public: SphericalCoordinates::SurfaceType surfaceType; diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index b3f67e198..a4042846b 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -16,9 +16,9 @@ */ #include -#include "ignition/math/SphericalCoordinates.hh" +#include "gz/math/SphericalCoordinates.hh" -using namespace ignition; +using namespace gz; ////////////////////////////////////////////////// // Test different constructors, default parameters diff --git a/src/Spline.cc b/src/Spline.cc index c54b3e39f..22e9e29a4 100644 --- a/src/Spline.cc +++ b/src/Spline.cc @@ -18,11 +18,11 @@ // spline and catmull-rom spline #include "SplinePrivate.hh" -#include "ignition/math/Helpers.hh" -#include "ignition/math/Vector4.hh" -#include "ignition/math/Spline.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Vector4.hh" +#include "gz/math/Spline.hh" -using namespace ignition; +using namespace gz; using namespace math; /////////////////////////////////////////////////////////// diff --git a/src/SplinePrivate.cc b/src/SplinePrivate.cc index 896b5af25..64a2b7c24 100644 --- a/src/SplinePrivate.cc +++ b/src/SplinePrivate.cc @@ -15,7 +15,7 @@ * */ -#include "ignition/math/Matrix4.hh" +#include "gz/math/Matrix4.hh" #include "SplinePrivate.hh" diff --git a/src/SplinePrivate.hh b/src/SplinePrivate.hh index ef82e2959..12a2b21ac 100644 --- a/src/SplinePrivate.hh +++ b/src/SplinePrivate.hh @@ -14,15 +14,15 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_SPLINEPRIVATE_HH_ -#define IGNITION_MATH_SPLINEPRIVATE_HH_ +#ifndef GZ_MATH_SPLINEPRIVATE_HH_ +#define GZ_MATH_SPLINEPRIVATE_HH_ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace ignition { diff --git a/src/Spline_TEST.cc b/src/Spline_TEST.cc index d1d013344..1cbd627f3 100644 --- a/src/Spline_TEST.cc +++ b/src/Spline_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Vector3.hh" -#include "ignition/math/Spline.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Spline.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(SplineTest, Spline) diff --git a/src/Stopwatch.cc b/src/Stopwatch.cc index d534125df..86701c059 100644 --- a/src/Stopwatch.cc +++ b/src/Stopwatch.cc @@ -15,12 +15,12 @@ * */ #include -#include "ignition/math/Stopwatch.hh" +#include "gz/math/Stopwatch.hh" -using namespace ignition::math; +using namespace gz::math; // Private data class -class ignition::math::StopwatchPrivate +class gz::math::StopwatchPrivate { /// \brief Default constructor. public: StopwatchPrivate() = default; diff --git a/src/Stopwatch_TEST.cc b/src/Stopwatch_TEST.cc index 0c5a69258..fc517e529 100644 --- a/src/Stopwatch_TEST.cc +++ b/src/Stopwatch_TEST.cc @@ -18,9 +18,9 @@ #include #include -#include "ignition/math/Stopwatch.hh" +#include "gz/math/Stopwatch.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// // Helper function that runs a few tests diff --git a/src/Temperature.cc b/src/Temperature.cc index 2fdf727a3..3a2e75f07 100644 --- a/src/Temperature.cc +++ b/src/Temperature.cc @@ -14,10 +14,10 @@ * limitations under the License. * */ -#include "ignition/math/Temperature.hh" +#include "gz/math/Temperature.hh" /// \brief Private data for the Temperature class. -class ignition::math::TemperaturePrivate +class gz::math::TemperaturePrivate { /// \brief Default constructor public: TemperaturePrivate() : kelvin(0.0) {} @@ -30,7 +30,7 @@ class ignition::math::TemperaturePrivate public: double kelvin; }; -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Temperature_TEST.cc b/src/Temperature_TEST.cc index ae3765673..c015476b8 100644 --- a/src/Temperature_TEST.cc +++ b/src/Temperature_TEST.cc @@ -16,9 +16,9 @@ */ #include -#include "ignition/math/Temperature.hh" +#include "gz/math/Temperature.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Triangle3_TEST.cc b/src/Triangle3_TEST.cc index bcc798ede..6748ab648 100644 --- a/src/Triangle3_TEST.cc +++ b/src/Triangle3_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Triangle3.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Triangle3.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; using namespace math; ///////////////////////////////////////////////// diff --git a/src/Triangle_TEST.cc b/src/Triangle_TEST.cc index 0e2e3372c..7581c39ed 100644 --- a/src/Triangle_TEST.cc +++ b/src/Triangle_TEST.cc @@ -17,10 +17,10 @@ #include -#include "ignition/math/Triangle.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Triangle.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(TriangleTest, Constructor) diff --git a/src/Vector2_TEST.cc b/src/Vector2_TEST.cc index 3e6e95f75..8ac6dabbc 100644 --- a/src/Vector2_TEST.cc +++ b/src/Vector2_TEST.cc @@ -18,10 +18,10 @@ #include #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Vector2.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Vector2.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Vector2Test, Construction) diff --git a/src/Vector3Stats.cc b/src/Vector3Stats.cc index 10ad3a248..be3c910ac 100644 --- a/src/Vector3Stats.cc +++ b/src/Vector3Stats.cc @@ -14,10 +14,10 @@ * limitations under the License. * */ -#include +#include #include "Vector3StatsPrivate.hh" -using namespace ignition; +using namespace gz; using namespace math; ////////////////////////////////////////////////// diff --git a/src/Vector3StatsPrivate.hh b/src/Vector3StatsPrivate.hh index 6715876a9..ecbcf3336 100644 --- a/src/Vector3StatsPrivate.hh +++ b/src/Vector3StatsPrivate.hh @@ -14,11 +14,11 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_VECTOR3STATSPRIVATE_HH_ -#define IGNITION_MATH_VECTOR3STATSPRIVATE_HH_ +#ifndef GZ_MATH_VECTOR3STATSPRIVATE_HH_ +#define GZ_MATH_VECTOR3STATSPRIVATE_HH_ -#include -#include +#include +#include namespace ignition { diff --git a/src/Vector3Stats_TEST.cc b/src/Vector3Stats_TEST.cc index 39fc5a5eb..b720e989e 100644 --- a/src/Vector3Stats_TEST.cc +++ b/src/Vector3Stats_TEST.cc @@ -17,9 +17,9 @@ #include -#include +#include -using namespace ignition; +using namespace gz; class Vector3StatsTest : public ::testing::Test { diff --git a/src/Vector3_TEST.cc b/src/Vector3_TEST.cc index 9a4615aa1..affade714 100644 --- a/src/Vector3_TEST.cc +++ b/src/Vector3_TEST.cc @@ -20,10 +20,10 @@ #include #include -#include "ignition/math/Vector3.hh" -#include "ignition/math/Helpers.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Helpers.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Vector3dTest, Construction) diff --git a/src/Vector4_TEST.cc b/src/Vector4_TEST.cc index 400a1f90c..216bc4a36 100644 --- a/src/Vector4_TEST.cc +++ b/src/Vector4_TEST.cc @@ -17,11 +17,11 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Matrix4.hh" -#include "ignition/math/Vector4.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Matrix4.hh" +#include "gz/math/Vector4.hh" -using namespace ignition; +using namespace gz; ///////////////////////////////////////////////// TEST(Vector4dTest, Construction) diff --git a/src/graph/Edge_TEST.cc b/src/graph/Edge_TEST.cc index ac6c1a247..85486b431 100644 --- a/src/graph/Edge_TEST.cc +++ b/src/graph/Edge_TEST.cc @@ -19,10 +19,10 @@ #include #include -#include "ignition/math/graph/Edge.hh" -#include "ignition/math/graph/Vertex.hh" +#include "gz/math/graph/Edge.hh" +#include "gz/math/graph/Vertex.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/graph/GraphAlgorithms_TEST.cc b/src/graph/GraphAlgorithms_TEST.cc index 87e238abb..1130f7e94 100644 --- a/src/graph/GraphAlgorithms_TEST.cc +++ b/src/graph/GraphAlgorithms_TEST.cc @@ -18,10 +18,10 @@ #include #include -#include "ignition/math/graph/Graph.hh" -#include "ignition/math/graph/GraphAlgorithms.hh" +#include "gz/math/graph/Graph.hh" +#include "gz/math/graph/GraphAlgorithms.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/graph/GraphDirected_TEST.cc b/src/graph/GraphDirected_TEST.cc index 81984f5aa..f9f338383 100644 --- a/src/graph/GraphDirected_TEST.cc +++ b/src/graph/GraphDirected_TEST.cc @@ -19,9 +19,9 @@ #include #include -#include "ignition/math/graph/Graph.hh" +#include "gz/math/graph/Graph.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/graph/GraphUndirected_TEST.cc b/src/graph/GraphUndirected_TEST.cc index dd34afc36..a49ee4337 100644 --- a/src/graph/GraphUndirected_TEST.cc +++ b/src/graph/GraphUndirected_TEST.cc @@ -21,9 +21,9 @@ #include #include -#include "ignition/math/graph/Graph.hh" +#include "gz/math/graph/Graph.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/graph/Graph_TEST.cc b/src/graph/Graph_TEST.cc index e58c9bdee..01fb7fbac 100644 --- a/src/graph/Graph_TEST.cc +++ b/src/graph/Graph_TEST.cc @@ -18,9 +18,9 @@ #include #include -#include "ignition/math/graph/Graph.hh" +#include "gz/math/graph/Graph.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/graph/Vertex_TEST.cc b/src/graph/Vertex_TEST.cc index 2bc0b0634..b0b9030ff 100644 --- a/src/graph/Vertex_TEST.cc +++ b/src/graph/Vertex_TEST.cc @@ -19,9 +19,9 @@ #include #include -#include "ignition/math/graph/Vertex.hh" +#include "gz/math/graph/Vertex.hh" -using namespace ignition; +using namespace gz; using namespace math; using namespace graph; diff --git a/src/python_pybind11/src/Angle.cc b/src/python_pybind11/src/Angle.cc index a0574c17e..5b632cb5a 100644 --- a/src/python_pybind11/src/Angle.cc +++ b/src/python_pybind11/src/Angle.cc @@ -17,7 +17,7 @@ #include -#include +#include #include #include "Angle.hh" @@ -32,7 +32,7 @@ namespace python { void defineMathAngle(py::module &m, const std::string &typestr) { - using Class = ignition::math::Angle; + using Class = gz::math::Angle; auto toString = [](const Class &si) { std::stringstream stream; stream << si; diff --git a/src/python_pybind11/src/Angle.hh b/src/python_pybind11/src/Angle.hh index f0b304d0e..f1d3a3714 100644 --- a/src/python_pybind11/src/Angle.hh +++ b/src/python_pybind11/src/Angle.hh @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Angle +/// Define a pybind11 wrapper for an gz::math::Angle /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python diff --git a/src/python_pybind11/src/AxisAlignedBox.cc b/src/python_pybind11/src/AxisAlignedBox.cc index 6ba253946..bb93abf38 100644 --- a/src/python_pybind11/src/AxisAlignedBox.cc +++ b/src/python_pybind11/src/AxisAlignedBox.cc @@ -21,8 +21,8 @@ #include "AxisAlignedBox.hh" -#include -#include +#include +#include using namespace pybind11::literals; @@ -34,7 +34,7 @@ namespace python { void defineMathAxisAlignedBox(py::module &m, const std::string &typestr) { - using Class = ignition::math::AxisAlignedBox; + using Class = gz::math::AxisAlignedBox; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -49,8 +49,8 @@ void defineMathAxisAlignedBox(py::module &m, const std::string &typestr) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def("x_length", &Class::XLength, "Get the length along the x dimension") @@ -74,8 +74,8 @@ void defineMathAxisAlignedBox(py::module &m, const std::string &typestr) "Get the volume of the box in m^3.") .def(py::self + py::self) .def(py::self += py::self) - .def(py::self + ignition::math::Vector3d()) - .def(py::self - ignition::math::Vector3d()) + .def(py::self + gz::math::Vector3d()) + .def(py::self - gz::math::Vector3d()) .def(py::self == py::self) .def(py::self != py::self) .def("min", diff --git a/src/python_pybind11/src/AxisAlignedBox.hh b/src/python_pybind11/src/AxisAlignedBox.hh index 057852353..87d3a7a43 100644 --- a/src/python_pybind11/src/AxisAlignedBox.hh +++ b/src/python_pybind11/src/AxisAlignedBox.hh @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::AxisAlignedBox +/// Define a pybind11 wrapper for an gz::math::AxisAlignedBox /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python diff --git a/src/python_pybind11/src/Box.hh b/src/python_pybind11/src/Box.hh index 49a7c79ab..72c53de5d 100644 --- a/src/python_pybind11/src/Box.hh +++ b/src/python_pybind11/src/Box.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__BOX_HH_ -#define IGNITION_MATH_PYTHON__BOX_HH_ +#ifndef GZ_MATH_PYTHON__BOX_HH_ +#define GZ_MATH_PYTHON__BOX_HH_ #include #include @@ -25,7 +25,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -36,7 +36,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Box +/// Define a pybind11 wrapper for an gz::math::Box /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -45,7 +45,7 @@ template void defineMathBox(py::module &m, const std::string &typestr) { - using Class = ignition::math::Box; + using Class = gz::math::Box; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -53,18 +53,18 @@ void defineMathBox(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init&>()) - .def(py::init&, - const ignition::math::Material&>()) + .def(py::init()) + .def(py::init()) + .def(py::init&>()) + .def(py::init&, + const gz::math::Material&>()) .def(py::self != py::self) .def(py::self == py::self) .def("size", &Class::Size, "Get the size of the box.") .def("set_size", - py::overload_cast&> + py::overload_cast&> (&Class::SetSize), "Set the size of the box.") .def("set_size", @@ -90,7 +90,7 @@ void defineMathBox(py::module &m, const std::string &typestr) .def("vertices_below", [](const Class &self, const Plane &_plane) { - std::vector> result; + std::vector> result; auto vertices = self.VerticesBelow(_plane); for (auto & v : vertices) { @@ -113,7 +113,7 @@ void defineMathBox(py::module &m, const std::string &typestr) .def("intersections", [](const Class &self, const Plane &_plane) { - std::vector> result; + std::vector> result; auto vertices = self.Intersections(_plane); for (auto & v : vertices) { @@ -135,4 +135,4 @@ void defineMathBox(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__BOX_HH_ +#endif // GZ_MATH_PYTHON__BOX_HH_ diff --git a/src/python_pybind11/src/Capsule.hh b/src/python_pybind11/src/Capsule.hh index fef96cfe4..7a4c57a7a 100644 --- a/src/python_pybind11/src/Capsule.hh +++ b/src/python_pybind11/src/Capsule.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__CAPSULE_HH_ -#define IGNITION_MATH_PYTHON__CAPSULE_HH_ +#ifndef GZ_MATH_PYTHON__CAPSULE_HH_ +#define GZ_MATH_PYTHON__CAPSULE_HH_ #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Capsule +/// Define a pybind11 wrapper for an gz::math::Capsule /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,7 +43,7 @@ namespace python template void helpDefineMathCapsule(py::module &m, const std::string &typestr) { - using Class = ignition::math::Capsule; + using Class = gz::math::Capsule; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -51,7 +51,7 @@ void helpDefineMathCapsule(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def(py::self == py::self) .def("radius", &Class::Radius, @@ -93,7 +93,7 @@ void helpDefineMathCapsule(py::module &m, const std::string &typestr) }, "memo"_a); } -/// Define a pybind11 wrapper for an ignition::math::Line2 +/// Define a pybind11 wrapper for an gz::math::Line2 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -104,4 +104,4 @@ void defineMathCapsule(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__BOX_HH_ +#endif // GZ_MATH_PYTHON__BOX_HH_ diff --git a/src/python_pybind11/src/Color.cc b/src/python_pybind11/src/Color.cc index 7fc0a2584..ba887f3aa 100644 --- a/src/python_pybind11/src/Color.cc +++ b/src/python_pybind11/src/Color.cc @@ -18,7 +18,7 @@ #include #include "Color.hh" -#include +#include #include @@ -32,7 +32,7 @@ namespace python { void defineMathColor(py::module &m, const std::string &typestr) { - using Class = ignition::math::Color; + using Class = gz::math::Color; auto toString = [](const Class &si) { std::stringstream stream; stream << si; diff --git a/src/python_pybind11/src/Color.hh b/src/python_pybind11/src/Color.hh index 7306bcb99..9986244a7 100644 --- a/src/python_pybind11/src/Color.hh +++ b/src/python_pybind11/src/Color.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__COLOR_HH_ -#define IGNITION_MATH_PYTHON__COLOR_HH_ +#ifndef GZ_MATH_PYTHON__COLOR_HH_ +#define GZ_MATH_PYTHON__COLOR_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Color +/// Define a pybind11 wrapper for an gz::math::Color /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathColor(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__COLOR_HH_ +#endif // GZ_MATH_PYTHON__COLOR_HH_ diff --git a/src/python_pybind11/src/Cylinder.hh b/src/python_pybind11/src/Cylinder.hh index eb7ecc722..8661829af 100644 --- a/src/python_pybind11/src/Cylinder.hh +++ b/src/python_pybind11/src/Cylinder.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__CYLINDER_HH_ -#define IGNITION_MATH_PYTHON__CYLINDER_HH_ +#ifndef GZ_MATH_PYTHON__CYLINDER_HH_ +#define GZ_MATH_PYTHON__CYLINDER_HH_ #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Cylinder +/// Define a pybind11 wrapper for an gz::math::Cylinder /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -44,7 +44,7 @@ template void defineMathCylinder(py::module &m, const std::string &typestr) { - using Class = ignition::math::Cylinder; + using Class = gz::math::Cylinder; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -52,17 +52,17 @@ void defineMathCylinder(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init&>(), + const gz::math::Quaternion&>(), py::arg("_length") = 0, py::arg("_radius") = 0, - py::arg("_rotOffset") = ignition::math::Quaternion::Identity) + py::arg("_rotOffset") = gz::math::Quaternion::Identity) .def(py::init&>(), + const gz::math::Material&, + const gz::math::Quaternion&>(), py::arg("_length") = 0, py::arg("_radius") = 0, - py::arg("_material") = ignition::math::Material(), - py::arg("_rotOffset") = ignition::math::Quaternion::Identity) + py::arg("_material") = gz::math::Material(), + py::arg("_rotOffset") = gz::math::Quaternion::Identity) .def(py::self == py::self) .def("radius", &Class::Radius, @@ -114,4 +114,4 @@ void defineMathCylinder(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__CYLINDER_HH_ +#endif // GZ_MATH_PYTHON__CYLINDER_HH_ diff --git a/src/python_pybind11/src/DiffDriveOdometry.cc b/src/python_pybind11/src/DiffDriveOdometry.cc index 025981181..672569cc6 100644 --- a/src/python_pybind11/src/DiffDriveOdometry.cc +++ b/src/python_pybind11/src/DiffDriveOdometry.cc @@ -26,7 +26,7 @@ namespace python void defineMathDiffDriveOdometry( py::module &m, const std::string &typestr) { - using Class = ignition::math::DiffDriveOdometry; + using Class = gz::math::DiffDriveOdometry; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -58,5 +58,5 @@ void defineMathDiffDriveOdometry( "Set the velocity rolling window size."); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/DiffDriveOdometry.hh b/src/python_pybind11/src/DiffDriveOdometry.hh index bc261df2d..9895f8c32 100644 --- a/src/python_pybind11/src/DiffDriveOdometry.hh +++ b/src/python_pybind11/src/DiffDriveOdometry.hh @@ -14,15 +14,15 @@ * limitations under the License. * */ -#ifndef IGNITION_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ -#define IGNITION_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ +#ifndef GZ_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ +#define GZ_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ #include #include #include #include -#include +#include namespace py = pybind11; @@ -32,14 +32,14 @@ namespace math { namespace python { -/// Define a py:: wrapper for an ignition::gazebo::DiffDriveOdometry +/// Define a py:: wrapper for an gz::sim::DiffDriveOdometry /** * \param[in] module a py:: module to add the definition to */ void defineMathDiffDriveOdometry( py::module &m, const std::string &typestr); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ +#endif // GZ_MATH_PYTHON__DIFFDRIVEODOMETRY_HH_ diff --git a/src/python_pybind11/src/Ellipsoid.hh b/src/python_pybind11/src/Ellipsoid.hh index 418cd17e0..c55e60869 100644 --- a/src/python_pybind11/src/Ellipsoid.hh +++ b/src/python_pybind11/src/Ellipsoid.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__Ellipsoid_HH_ -#define IGNITION_MATH_PYTHON__Ellipsoid_HH_ +#ifndef GZ_MATH_PYTHON__Ellipsoid_HH_ +#define GZ_MATH_PYTHON__Ellipsoid_HH_ #include @@ -24,8 +24,8 @@ #include #include -#include -#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -36,7 +36,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Ellipsoid +/// Define a pybind11 wrapper for an gz::math::Ellipsoid /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -44,16 +44,16 @@ namespace python template void helpDefineMathEllipsoid(py::module &m, const std::string &typestr) { - using Class = ignition::math::Ellipsoid; + using Class = gz::math::Ellipsoid; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init>()) - .def(py::init, - const ignition::math::Material&>()) + .def(py::init>()) + .def(py::init, + const gz::math::Material&>()) .def(py::self == py::self) .def("radii", &Class::Radii, @@ -89,7 +89,7 @@ void helpDefineMathEllipsoid(py::module &m, const std::string &typestr) }, "memo"_a); } -/// Define a pybind11 wrapper for an ignition::math::Ellipsoid +/// Define a pybind11 wrapper for an gz::math::Ellipsoid /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -100,4 +100,4 @@ void defineMathEllipsoid(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__BOX_HH_ +#endif // GZ_MATH_PYTHON__BOX_HH_ diff --git a/src/python_pybind11/src/Filter.cc b/src/python_pybind11/src/Filter.cc index 93abe5c60..e40559372 100644 --- a/src/python_pybind11/src/Filter.cc +++ b/src/python_pybind11/src/Filter.cc @@ -39,7 +39,7 @@ void defineMathOnePole(py::module &m, const std::string &typestr) void defineMathOnePoleQuaternion(py::module &m, const std::string &typestr) { - using Class = ignition::math::OnePoleQuaternion; + using Class = gz::math::OnePoleQuaternion; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -63,7 +63,7 @@ void defineMathOnePoleQuaternion(py::module &m, const std::string &typestr) void defineMathOnePoleVector3(py::module &m, const std::string &typestr) { - using Class = ignition::math::OnePoleVector3; + using Class = gz::math::OnePoleVector3; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -95,7 +95,7 @@ void defineMathBiQuad(py::module &m, const std::string &typestr) void defineMathBiQuadVector3(py::module &m, const std::string &typestr) { - using Class = ignition::math::BiQuadVector3; + using Class = gz::math::BiQuadVector3; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/Filter.hh b/src/python_pybind11/src/Filter.hh index df8b59e80..4b33118a7 100644 --- a/src/python_pybind11/src/Filter.hh +++ b/src/python_pybind11/src/Filter.hh @@ -15,16 +15,16 @@ * */ -#ifndef IGNITION_MATH_PYTHON__FILTER_HH_ -#define IGNITION_MATH_PYTHON__FILTER_HH_ +#ifndef GZ_MATH_PYTHON__FILTER_HH_ +#define GZ_MATH_PYTHON__FILTER_HH_ #include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; @@ -71,7 +71,7 @@ public: } }; -/// Help define a pybind11 wrapper for an ignition::math::Filter +/// Help define a pybind11 wrapper for an gz::math::Filter /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -79,7 +79,7 @@ public: template void helpDefineMathFilter(py::module &m, const std::string &typestr) { - using Class = ignition::math::Filter; + using Class = gz::math::Filter; std::string pyclass_name = typestr; py::class_>(m, pyclass_name.c_str(), @@ -97,7 +97,7 @@ void helpDefineMathFilter(py::module &m, const std::string &typestr) "Get the output of the filter."); } -/// Define a pybind11 wrapper for an ignition::math::Filter +/// Define a pybind11 wrapper for an gz::math::Filter /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -123,7 +123,7 @@ public: } }; -/// Help define a pybind11 wrapper for an ignition::math::OnePole +/// Help define a pybind11 wrapper for an gz::math::OnePole /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -131,7 +131,7 @@ public: template void helpDefineMathOnePole(py::module &m, const std::string &typestr) { - using Class = ignition::math::OnePole; + using Class = gz::math::OnePole; std::string pyclass_name = typestr; py::class_>(m, pyclass_name.c_str(), @@ -153,21 +153,21 @@ void helpDefineMathOnePole(py::module &m, const std::string &typestr) "Update the filter's output."); } -/// Define a pybind11 wrapper for an ignition::math::OnePole +/// Define a pybind11 wrapper for an gz::math::OnePole /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathOnePole(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::OnePoleQuaterion +/// Define a pybind11 wrapper for an gz::math::OnePoleQuaterion /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathOnePoleQuaternion(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::OnePoleVector3 +/// Define a pybind11 wrapper for an gz::math::OnePoleVector3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -213,7 +213,7 @@ public: } }; -/// Help define a pybind11 wrapper for an ignition::math::BiQuad +/// Help define a pybind11 wrapper for an gz::math::BiQuad /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -221,7 +221,7 @@ public: template void helpDefineMathBiQuad(py::module &m, const std::string &typestr) { - using Class = ignition::math::BiQuad; + using Class = gz::math::BiQuad; std::string pyclass_name = typestr; py::class_>(m, pyclass_name.c_str(), @@ -246,14 +246,14 @@ void helpDefineMathBiQuad(py::module &m, const std::string &typestr) "Update the filter's output."); } -/// Define a pybind11 wrapper for an ignition::math::BiQuad +/// Define a pybind11 wrapper for an gz::math::BiQuad /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathBiQuad(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::BiQuadVector3 +/// Define a pybind11 wrapper for an gz::math::BiQuadVector3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -264,4 +264,4 @@ void defineMathBiQuadVector3(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__FILTER_HH_ +#endif // GZ_MATH_PYTHON__FILTER_HH_ diff --git a/src/python_pybind11/src/Frustum.cc b/src/python_pybind11/src/Frustum.cc index 7434ff440..70ed41319 100644 --- a/src/python_pybind11/src/Frustum.cc +++ b/src/python_pybind11/src/Frustum.cc @@ -17,7 +17,7 @@ #include #include "Frustum.hh" -#include +#include #include #include @@ -30,20 +30,20 @@ namespace python { void defineMathFrustum(py::module &m, const std::string &typestr) { - using Class = ignition::math::Frustum; + using Class = gz::math::Frustum; std::string pyclass_name = typestr; py::class_ (m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init(), + .def(py::init(), py::arg("_near") = 0, py::arg("_far") = 0, - py::arg("_fov") = ignition::math::Angle(0), + py::arg("_fov") = gz::math::Angle(0), py::arg("_aspectRatio") = 0, - py::arg("_pose") = ignition::math::Pose3d::Zero) + py::arg("_pose") = gz::math::Pose3d::Zero) .def(py::init()) .def("near", &Class::Near, @@ -84,30 +84,30 @@ void defineMathFrustum(py::module &m, const std::string &typestr) &Class::SetPose, "Set the pose of the frustum") .def("contains", - py::overload_cast + py::overload_cast (&Class::Contains, py::const_), "Check if a box lies inside the pyramid frustum.") .def("contains", - py::overload_cast + py::overload_cast (&Class::Contains, py::const_), "Check if a point lies inside the pyramid frustum.") .def("plane", &Class::Plane, "Get a plane of the frustum."); - py::enum_(m, "FrustumPlane") + py::enum_(m, "FrustumPlane") .value("FRUSTUM_PLANE_NEAR", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_NEAR) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_NEAR) .value("FRUSTUM_PLANE_FAR", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_FAR) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_FAR) .value("FRUSTUM_PLANE_LEFT", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_LEFT) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_LEFT) .value("FRUSTUM_PLANE_RIGHT", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_RIGHT) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_RIGHT) .value("FRUSTUM_PLANE_TOP", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_TOP) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_TOP) .value("FRUSTUM_PLANE_BOTTOM", - ignition::math::Frustum::FrustumPlane::FRUSTUM_PLANE_BOTTOM) + gz::math::Frustum::FrustumPlane::FRUSTUM_PLANE_BOTTOM) .export_values(); } } // namespace python diff --git a/src/python_pybind11/src/Frustum.hh b/src/python_pybind11/src/Frustum.hh index 688baf013..1a9723e52 100644 --- a/src/python_pybind11/src/Frustum.hh +++ b/src/python_pybind11/src/Frustum.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__FRUSTUM_HH_ -#define IGNITION_MATH_PYTHON__FRUSTUM_HH_ +#ifndef GZ_MATH_PYTHON__FRUSTUM_HH_ +#define GZ_MATH_PYTHON__FRUSTUM_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Frustum +/// Define a pybind11 wrapper for an gz::math::Frustum /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathFrustum(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__FRUSTUM_HH_ +#endif // GZ_MATH_PYTHON__FRUSTUM_HH_ diff --git a/src/python_pybind11/src/GaussMarkovProcess.cc b/src/python_pybind11/src/GaussMarkovProcess.cc index 928744409..3b218b2e2 100644 --- a/src/python_pybind11/src/GaussMarkovProcess.cc +++ b/src/python_pybind11/src/GaussMarkovProcess.cc @@ -22,7 +22,7 @@ #include #include -#include +#include #include "GaussMarkovProcess.hh" @@ -37,7 +37,7 @@ namespace python void defineMathGaussMarkovProcess( py::module &m, const std::string &typestr) { - using Class = ignition::math::GaussMarkovProcess; + using Class = gz::math::GaussMarkovProcess; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/GaussMarkovProcess.hh b/src/python_pybind11/src/GaussMarkovProcess.hh index c100c684f..66a21f006 100644 --- a/src/python_pybind11/src/GaussMarkovProcess.hh +++ b/src/python_pybind11/src/GaussMarkovProcess.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ -#define IGNITION_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ +#ifndef GZ_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ +#define GZ_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ #include @@ -30,7 +30,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::GaussMarkovProcess +/// Define a pybind11 wrapper for an gz::math::GaussMarkovProcess /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -40,4 +40,4 @@ void defineMathGaussMarkovProcess(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ +#endif // GZ_MATH_PYTHON__GAUSSMARKOVPROCESS_HH_ diff --git a/src/python_pybind11/src/Helpers.cc b/src/python_pybind11/src/Helpers.cc index dcd0934e8..c4d3a2a6f 100644 --- a/src/python_pybind11/src/Helpers.cc +++ b/src/python_pybind11/src/Helpers.cc @@ -19,8 +19,8 @@ #include #include "Helpers.hh" -#include -#include +#include +#include namespace ignition { @@ -62,7 +62,7 @@ float BoxVolume(const float _x, const float _y, const float _z) /// \brief Compute box volume from a vector /// \param[in] _v Vector3d that contains the box's dimensions. /// \return box volume from a vector -float BoxVolumeV(const ignition::math::Vector3d &_v) +float BoxVolumeV(const gz::math::Vector3d &_v) { return IGN_BOX_VOLUME_V(_v); } @@ -92,55 +92,55 @@ void defineMathHelpers(py::module &m) { using Class = Helpers; - m.def("clamp", &ignition::math::clamp, "Simple clamping function") - .def("clamp", &ignition::math::clamp, "Simple clamping function") + m.def("clamp", &gz::math::clamp, "Simple clamping function") + .def("clamp", &gz::math::clamp, "Simple clamping function") .def("isnan", - py::overload_cast(&ignition::math::isnan), + py::overload_cast(&gz::math::isnan), "Check if a float is NaN") .def("fixnan", - py::overload_cast(&ignition::math::fixnan), + py::overload_cast(&gz::math::fixnan), "Fix a nan value.") .def("is_even", - py::overload_cast(&ignition::math::isEven), + py::overload_cast(&gz::math::isEven), "Check if parameter is even.") .def("is_odd", - py::overload_cast(&ignition::math::isOdd), + py::overload_cast(&gz::math::isOdd), "Check if parameter is odd.") .def("sgn", - &ignition::math::sgn, + &gz::math::sgn, "Returns 0 for zero values, -1 for negative values and +1 for positive" " values.") .def("signum", - &ignition::math::signum, + &gz::math::signum, "Returns 0 for zero values, -1 for negative values and " "+1 for positive values.") - .def("mean", &ignition::math::mean, "Get mean of vector of values") + .def("mean", &gz::math::mean, "Get mean of vector of values") .def("variance", - &ignition::math::variance, + &gz::math::variance, "Get variance of vector of values") .def("max", - &ignition::math::max, + &gz::math::max, "Get the maximum value of vector of values") .def("max", - &ignition::math::max, + &gz::math::max, "Get the maximum value of vector of values") .def("min", - &ignition::math::min, + &gz::math::min, "Get the minimum value of vector of values") .def("min", - &ignition::math::min, + &gz::math::min, "Get the minimum value of vector of values") .def("equal", - &ignition::math::equal, + &gz::math::equal, "check if two values are equal, within a tolerance") .def("less_or_near_equal", - &ignition::math::lessOrNearEqual, + &gz::math::lessOrNearEqual, "Inequality test, within a tolerance") .def("greater_or_near_equal", - &ignition::math::greaterOrNearEqual, + &gz::math::greaterOrNearEqual, "Inequality test, within a tolerance") .def("precision", - &ignition::math::precision, + &gz::math::precision, "Get value at a specified precision") .def("sort2", &Sort2, @@ -149,23 +149,23 @@ void defineMathHelpers(py::module &m) &Sort3, "Sort three numbers, such that _a <= _b <= _c") .def("is_power_of_two", - &ignition::math::isPowerOfTwo, + &gz::math::isPowerOfTwo, "Is this a power of 2?") .def("round_up_power_of_two", - &ignition::math::roundUpPowerOfTwo, + &gz::math::roundUpPowerOfTwo, "Get the smallest power of two that is greater or equal to a given " "value") .def("round_up_multiple", - &ignition::math::roundUpMultiple, + &gz::math::roundUpMultiple, "Round a number up to the nearest multiple") .def("parse_int", - &ignition::math::parseInt, + &gz::math::parseInt, "parse string into an integer") .def("parse_float", - &ignition::math::parseFloat, + &gz::math::parseFloat, "parse string into an float") .def("is_time_string", - &ignition::math::isTimeString, + &gz::math::isTimeString, "An example time string is \"0 00:00:00.000\", which has the format " "DAYS HOURS:MINUTES:SECONDS.MILLISECONDS") .def("ign_sphere_volume", @@ -231,5 +231,5 @@ void defineMathHelpers(py::module &m) .def_readonly_static("NAN_I", &NAN_I); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Helpers.hh b/src/python_pybind11/src/Helpers.hh index ed22bd325..c83bd4b59 100644 --- a/src/python_pybind11/src/Helpers.hh +++ b/src/python_pybind11/src/Helpers.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__HELPERS_HH_ -#define IGNITION_MATH_PYTHON__HELPERS_HH_ +#ifndef GZ_MATH_PYTHON__HELPERS_HH_ +#define GZ_MATH_PYTHON__HELPERS_HH_ #include @@ -28,13 +28,13 @@ namespace math { namespace python { -/// Define a py::bind11 wrapper for an ignition::math::Helpers +/// Define a py::bind11 wrapper for an gz::math::Helpers /** * \param[in] module a py::bind11 module to add the definition to */ void defineMathHelpers(py::module &m); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__HELPERS_HH_ +#endif // GZ_MATH_PYTHON__HELPERS_HH_ diff --git a/src/python_pybind11/src/Inertial.hh b/src/python_pybind11/src/Inertial.hh index 94b1de7b3..54c6fe70c 100644 --- a/src/python_pybind11/src/Inertial.hh +++ b/src/python_pybind11/src/Inertial.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__INERTIAL_HH_ -#define IGNITION_MATH_PYTHON__INERTIAL_HH_ +#ifndef GZ_MATH_PYTHON__INERTIAL_HH_ +#define GZ_MATH_PYTHON__INERTIAL_HH_ #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Inertial +/// Define a pybind11 wrapper for an gz::math::Inertial /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -44,15 +44,15 @@ template void defineMathInertial(py::module &m, const std::string &typestr) { - using Class = ignition::math::Inertial; + using Class = gz::math::Inertial; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init&, - const ignition::math::Pose3&>()) + .def(py::init&, + const gz::math::Pose3&>()) .def(py::init()) .def(py::self == py::self) .def(py::self != py::self) @@ -60,7 +60,7 @@ void defineMathInertial(py::module &m, const std::string &typestr) .def(py::self + py::self) .def("set_mass_matrix", &Class::SetMassMatrix, - py::arg("_m") = ignition::math::MassMatrix3(), + py::arg("_m") = gz::math::MassMatrix3(), py::arg("_tolerance") = IGN_MASSMATRIX3_DEFAULT_TOLERANCE, "Set the mass and inertia matrix.") .def("mass_matrix", @@ -82,7 +82,7 @@ void defineMathInertial(py::module &m, const std::string &typestr) "MOI in the base coordinate frame.") .def("set_mass_matrix_rotation", &Class::SetMassMatrixRotation, - py::arg("_q") = ignition::math::Quaternion::Identity, + py::arg("_q") = gz::math::Quaternion::Identity, py::arg("_tol") = 1e-6, "Set the MassMatrix rotation (eigenvectors of inertia matrix) " "without affecting the MOI in the base coordinate frame.") @@ -98,4 +98,4 @@ void defineMathInertial(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__INERTIAL_HH_ +#endif // GZ_MATH_PYTHON__INERTIAL_HH_ diff --git a/src/python_pybind11/src/Interval.hh b/src/python_pybind11/src/Interval.hh index 7539bff31..2a1c70744 100644 --- a/src/python_pybind11/src/Interval.hh +++ b/src/python_pybind11/src/Interval.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__INTERVAL_HH_ -#define IGNITION_MATH_PYTHON__INTERVAL_HH_ +#ifndef GZ_MATH_PYTHON__INTERVAL_HH_ +#define GZ_MATH_PYTHON__INTERVAL_HH_ #include #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Interval +/// Help define a pybind11 wrapper for an gz::math::Interval /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,7 +43,7 @@ namespace python template void helpDefineMathInterval(py::module &m, const std::string &typestr) { - using Class = ignition::math::Interval; + using Class = gz::math::Interval; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -104,7 +104,7 @@ void helpDefineMathInterval(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Interval +/// Define a pybind11 wrapper for an gz::math::Interval /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -115,4 +115,4 @@ void defineMathInterval(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__INTERVAL_HH_ +#endif // GZ_MATH_PYTHON__INTERVAL_HH_ diff --git a/src/python_pybind11/src/Kmeans.cc b/src/python_pybind11/src/Kmeans.cc index 13af747dc..78f028f4f 100644 --- a/src/python_pybind11/src/Kmeans.cc +++ b/src/python_pybind11/src/Kmeans.cc @@ -18,7 +18,7 @@ #include #include "Kmeans.hh" -#include +#include #include namespace ignition @@ -29,15 +29,15 @@ namespace python { void defineMathKmeans(py::module &m, const std::string &typestr) { - using Class = ignition::math::Kmeans; + using Class = gz::math::Kmeans; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) - .def(py::init&>()) + .def(py::init&>()) .def("observations", - py::overload_cast&> + py::overload_cast&> (&Class::Observations), "Set the observations to cluster.") .def("observations", @@ -48,7 +48,7 @@ void defineMathKmeans(py::module &m, const std::string &typestr) "Add observations to the cluster.") .def("cluster", [](Class &self, int k) { - std::vector> centroids; + std::vector> centroids; std::vector labels; bool result = self.Cluster(k, centroids, labels); return std::make_tuple(result, centroids, labels); diff --git a/src/python_pybind11/src/Kmeans.hh b/src/python_pybind11/src/Kmeans.hh index d9c1de4e0..030b2c998 100644 --- a/src/python_pybind11/src/Kmeans.hh +++ b/src/python_pybind11/src/Kmeans.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__KMEANS_HH_ -#define IGNITION_MATH_PYTHON__KMEANS_HH_ +#ifndef GZ_MATH_PYTHON__KMEANS_HH_ +#define GZ_MATH_PYTHON__KMEANS_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Kmeans +/// Define a pybind11 wrapper for an gz::math::Kmeans /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathKmeans(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__KMEANS_HH_ +#endif // GZ_MATH_PYTHON__KMEANS_HH_ diff --git a/src/python_pybind11/src/Line2.hh b/src/python_pybind11/src/Line2.hh index 351035443..fc06b7e85 100644 --- a/src/python_pybind11/src/Line2.hh +++ b/src/python_pybind11/src/Line2.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__LINE2_HH_ -#define IGNITION_MATH_PYTHON__LINE2_HH_ +#ifndef GZ_MATH_PYTHON__LINE2_HH_ +#define GZ_MATH_PYTHON__LINE2_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,7 +34,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Line2 +/// Help define a pybind11 wrapper for an gz::math::Line2 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -42,7 +42,7 @@ namespace python template void helpDefineMathLine2(py::module &m, const std::string &typestr) { - using Class = ignition::math::Line2; + using Class = gz::math::Line2; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -53,14 +53,14 @@ void helpDefineMathLine2(py::module &m, const std::string &typestr) pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) - .def(py::init&, - const ignition::math::Vector2&>()) + .def(py::init&, + const gz::math::Vector2&>()) .def(py::init()) .def(py::self != py::self) .def(py::self == py::self) .def("set", - py::overload_cast&, - const ignition::math::Vector2&>(&Class::Set), + py::overload_cast&, + const gz::math::Vector2&>(&Class::Set), "Set the start and end point of the line segment") .def("set", py::overload_cast(&Class::Set), @@ -69,13 +69,13 @@ void helpDefineMathLine2(py::module &m, const std::string &typestr) py::overload_cast(&Class::CrossProduct, py::const_), "Return the cross product of this line and the given line.") .def("cross_product", - py::overload_cast&>( + py::overload_cast&>( &Class::CrossProduct, py::const_), "Return the cross product of this line and the given line.") .def("collinear", - py::overload_cast&, double>( + py::overload_cast&, double>( &Class::Collinear, py::const_), - py::arg("_pt") = ignition::math::Vector2::Zero, + py::arg("_pt") = gz::math::Vector2::Zero, py::arg("_epsilon") = 1e-6, "Check if the given point is collinear with this line.") .def("collinear", @@ -97,11 +97,11 @@ void helpDefineMathLine2(py::module &m, const std::string &typestr) .def("intersect", py::overload_cast< const Class&, - ignition::math::Vector2&, + gz::math::Vector2&, double>( &Class::Intersect, py::const_), py::arg("_line") = Class(0, 0, 0, 0), - py::arg("_pt") = ignition::math::Vector2::Zero, + py::arg("_pt") = gz::math::Vector2::Zero, py::arg("_epsilon") = 1e-6, "Check if this line intersects the given line segment.") .def("intersect", @@ -128,7 +128,7 @@ void helpDefineMathLine2(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Line2 +/// Define a pybind11 wrapper for an gz::math::Line2 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -139,4 +139,4 @@ void defineMathLine2(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__LINE2_HH_ +#endif // GZ_MATH_PYTHON__LINE2_HH_ diff --git a/src/python_pybind11/src/Line3.hh b/src/python_pybind11/src/Line3.hh index 854ee8f43..3da0f8376 100644 --- a/src/python_pybind11/src/Line3.hh +++ b/src/python_pybind11/src/Line3.hh @@ -15,16 +15,16 @@ * */ -#ifndef IGNITION_MATH_PYTHON__LINE3_HH_ -#define IGNITION_MATH_PYTHON__LINE3_HH_ +#ifndef GZ_MATH_PYTHON__LINE3_HH_ +#define GZ_MATH_PYTHON__LINE3_HH_ #include #include #include -#include -#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Line3 +/// Help define a pybind11 wrapper for an gz::math::Line3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,7 +43,7 @@ namespace python template void helpDefineMathLine3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Line3; + using Class = gz::math::Line3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -56,8 +56,8 @@ void helpDefineMathLine3(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init()) - .def(py::init&, - const ignition::math::Vector3&>(), + .def(py::init&, + const gz::math::Vector3&>(), "Constructor") .def(py::init(), "2D Constructor where Z coordinates are 0") @@ -67,8 +67,8 @@ void helpDefineMathLine3(py::module &m, const std::string &typestr) .def(py::self != py::self) .def(py::self == py::self) .def("set", - py::overload_cast&, - const ignition::math::Vector3&>(&Class::Set), + py::overload_cast&, + const gz::math::Vector3&>(&Class::Set), "Set the start and end point of the line segment") .def("set_a", &Class::SetA, @@ -100,17 +100,17 @@ void helpDefineMathLine3(py::module &m, const std::string &typestr) py::arg("_line"), py::arg("_result"), py::arg("_epsilon") = 1e-6, "Get the shortest line between this line and the provided line.") .def("distance", - py::overload_cast&>( + py::overload_cast&>( &Class::Distance), "Calculate shortest distance between line and point") .def("intersect", py::overload_cast< const Class&, - ignition::math::Vector3&, + gz::math::Vector3&, double>( &Class::Intersect, py::const_), py::arg("_line") = Class(0, 0, 0, 0), - py::arg("_pt") = ignition::math::Vector3::Zero, + py::arg("_pt") = gz::math::Vector3::Zero, py::arg("_epsilon") = 1e-6, "Check if this line intersects the given line segment.") .def("intersect", @@ -127,7 +127,7 @@ void helpDefineMathLine3(py::module &m, const std::string &typestr) "Check if the given line is parallel with this line.") .def("within", &Class::Within, - py::arg("_pt") = ignition::math::Vector3::Zero, + py::arg("_pt") = gz::math::Vector3::Zero, py::arg("_epsilon") = 1e-6, "Check if the given point is between the start and end " "points of the line segment.") @@ -145,7 +145,7 @@ void helpDefineMathLine3(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Line3 +/// Define a pybind11 wrapper for an gz::math::Line3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -155,4 +155,4 @@ void defineMathLine3(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__LINE3_HH_ +#endif // GZ_MATH_PYTHON__LINE3_HH_ diff --git a/src/python_pybind11/src/MassMatrix3.cc b/src/python_pybind11/src/MassMatrix3.cc index b7595bdf1..ad9c647f6 100644 --- a/src/python_pybind11/src/MassMatrix3.cc +++ b/src/python_pybind11/src/MassMatrix3.cc @@ -32,5 +32,5 @@ void defineMathMassMatrix3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/MassMatrix3.hh b/src/python_pybind11/src/MassMatrix3.hh index 97f9e830f..2650a446c 100644 --- a/src/python_pybind11/src/MassMatrix3.hh +++ b/src/python_pybind11/src/MassMatrix3.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__MASSMATRIX3_HH_ -#define IGNITION_MATH_PYTHON__MASSMATRIX3_HH_ +#ifndef GZ_MATH_PYTHON__MASSMATRIX3_HH_ +#define GZ_MATH_PYTHON__MASSMATRIX3_HH_ #include #include #include -#include -#include +#include +#include namespace py = pybind11; @@ -33,14 +33,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::MassMatrix3 +/// Define a pybind11 wrapper for an gz::math::MassMatrix3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathMassMatrix3(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::MassMatrix3 +/// Help define a pybind11 wrapper for an gz::math::MassMatrix3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -48,7 +48,7 @@ void defineMathMassMatrix3(py::module &m, const std::string &typestr); template void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) { - using Class = ignition::math::MassMatrix3; + using Class = gz::math::MassMatrix3; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -56,8 +56,8 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init()) - .def(py::init&, - const ignition::math::Vector3&>()) + .def(py::init&, + const gz::math::Vector3&>()) .def("set_mass", &Class::SetMass, "Set the mass.") .def("mass", py::overload_cast<>(&Class::Mass, py::const_), "Get the mass") .def("ixx", &Class::Ixx, "Get ixx") @@ -92,55 +92,55 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) &Class::SetDiagonalMoments, "Set the diagonal moments of inertia (Ixx, Iyy, Izz).") .def("set_from_box", - py::overload_cast&, - const ignition::math::Quaternion&> + py::overload_cast&, + const gz::math::Quaternion&> (&Class::SetFromBox), - py::arg("_mat") = ignition::math::Material(), - py::arg("_size") = ignition::math::Vector3::Zero, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_mat") = gz::math::Material(), + py::arg("_size") = gz::math::Vector3::Zero, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent box.") .def("set_from_box", - py::overload_cast&, - const ignition::math::Quaternion&> + py::overload_cast&, + const gz::math::Quaternion&> (&Class::SetFromBox), py::arg("_mass") = 0, - py::arg("_size") = ignition::math::Vector3::Zero, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_size") = gz::math::Vector3::Zero, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent box.") .def("set_from_box", - py::overload_cast&, - const ignition::math::Quaternion&> + py::overload_cast&, + const gz::math::Quaternion&> (&Class::SetFromBox), - py::arg("_size") = ignition::math::Vector3::Zero, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_size") = gz::math::Vector3::Zero, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent box.") .def("set_from_cylinder_z", py::overload_cast&> + const gz::math::Quaternion&> (&Class::SetFromCylinderZ), - py::arg("_mat") = ignition::math::Material(), + py::arg("_mat") = gz::math::Material(), py::arg("_length") = 0, py::arg("_radius") = 0, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent " "cylinder aligned with Z axis.") .def("set_from_cylinder_z", py::overload_cast&> + const gz::math::Quaternion&> (&Class::SetFromCylinderZ), py::arg("_mass") = 0, py::arg("_length") = 0, py::arg("_radius") = 0, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent " "cylinder aligned with Z axis.") .def("set_from_cylinder_z", py::overload_cast&> + const gz::math::Quaternion&> (&Class::SetFromCylinderZ), py::arg("_length") = 0, py::arg("_radius") = 0, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent " "cylinder aligned with Z axis.") .def("set_from_sphere", @@ -159,8 +159,8 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) "using the current mass value.") .def("equivalent_box", &Class::EquivalentBox, - py::arg("_size") = ignition::math::Vector3::Zero, - py::arg("_rot") = ignition::math::Quaternion::Identity, + py::arg("_size") = gz::math::Vector3::Zero, + py::arg("_rot") = gz::math::Quaternion::Identity, py::arg("_tol") = 1e-6, "Get dimensions and rotation offset of uniform box " "with equivalent mass and moment of inertia.") @@ -182,7 +182,7 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) "Verify that inertia values are positive semi-definite " "and satisfy the triangle inequality.") .def("epsilon", - py::overload_cast&, const T> + py::overload_cast&, const T> (&Class::Epsilon), py::arg("_tolerance") = IGN_MASSMATRIX3_DEFAULT_TOLERANCE, "Get an epsilon value that represents the amount of " @@ -204,4 +204,4 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__MASSMATRIX3_HH_ +#endif // GZ_MATH_PYTHON__MASSMATRIX3_HH_ diff --git a/src/python_pybind11/src/Material.cc b/src/python_pybind11/src/Material.cc index 929293e97..1b215d90d 100644 --- a/src/python_pybind11/src/Material.cc +++ b/src/python_pybind11/src/Material.cc @@ -19,8 +19,8 @@ #include #include "Material.hh" -#include -#include +#include +#include #include #include @@ -35,17 +35,17 @@ void defineMathMaterial(py::module &m, const std::string &typestr) { py::bind_map> + gz::math::MaterialType, gz::math::Material>> (m, "MaterialMap"); - using Class = ignition::math::Material; + using Class = gz::math::Material; std::string pyclass_name = typestr; py::class_ (m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init()) + .def(py::init()) .def(py::init()) .def(py::init()) .def(py::init()) @@ -83,22 +83,22 @@ void defineMathMaterial(py::module &m, const std::string &typestr) .def("density", &Class::Density, "Set the density value of the material in kg/m^3."); - py::enum_(m, "MaterialType") - .value("STYROFOAM", ignition::math::MaterialType::STYROFOAM) - .value("PINE", ignition::math::MaterialType::PINE) - .value("WOOD", ignition::math::MaterialType::WOOD) - .value("OAK", ignition::math::MaterialType::OAK) - .value("PLASTIC", ignition::math::MaterialType::PLASTIC) - .value("CONCRETE", ignition::math::MaterialType::CONCRETE) - .value("ALUMINUM", ignition::math::MaterialType::ALUMINUM) - .value("STEEL_ALLOY", ignition::math::MaterialType::STEEL_ALLOY) - .value("STEEL_STAINLESS", ignition::math::MaterialType::STEEL_STAINLESS) - .value("IRON", ignition::math::MaterialType::IRON) - .value("BRASS", ignition::math::MaterialType::BRASS) - .value("COPPER", ignition::math::MaterialType::COPPER) - .value("TUNGSTEN", ignition::math::MaterialType::TUNGSTEN) + py::enum_(m, "MaterialType") + .value("STYROFOAM", gz::math::MaterialType::STYROFOAM) + .value("PINE", gz::math::MaterialType::PINE) + .value("WOOD", gz::math::MaterialType::WOOD) + .value("OAK", gz::math::MaterialType::OAK) + .value("PLASTIC", gz::math::MaterialType::PLASTIC) + .value("CONCRETE", gz::math::MaterialType::CONCRETE) + .value("ALUMINUM", gz::math::MaterialType::ALUMINUM) + .value("STEEL_ALLOY", gz::math::MaterialType::STEEL_ALLOY) + .value("STEEL_STAINLESS", gz::math::MaterialType::STEEL_STAINLESS) + .value("IRON", gz::math::MaterialType::IRON) + .value("BRASS", gz::math::MaterialType::BRASS) + .value("COPPER", gz::math::MaterialType::COPPER) + .value("TUNGSTEN", gz::math::MaterialType::TUNGSTEN) .value("UNKNOWN_MATERIAL", - ignition::math::MaterialType::UNKNOWN_MATERIAL) + gz::math::MaterialType::UNKNOWN_MATERIAL) .export_values(); } } // namespace python diff --git a/src/python_pybind11/src/Material.hh b/src/python_pybind11/src/Material.hh index a8029c707..163cf5120 100644 --- a/src/python_pybind11/src/Material.hh +++ b/src/python_pybind11/src/Material.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__MATERIAL_HH_ -#define IGNITION_MATH_PYTHON__MATERIAL_HH_ +#ifndef GZ_MATH_PYTHON__MATERIAL_HH_ +#define GZ_MATH_PYTHON__MATERIAL_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Material +/// Define a pybind11 wrapper for an gz::math::Material /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathMaterial(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__MATERIAL_HH_ +#endif // GZ_MATH_PYTHON__MATERIAL_HH_ diff --git a/src/python_pybind11/src/Matrix3.cc b/src/python_pybind11/src/Matrix3.cc index e987a96db..011bf3410 100644 --- a/src/python_pybind11/src/Matrix3.cc +++ b/src/python_pybind11/src/Matrix3.cc @@ -33,5 +33,5 @@ void defineMathMatrix3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Matrix3.hh b/src/python_pybind11/src/Matrix3.hh index bd2cb6a1a..8b3c5e85e 100644 --- a/src/python_pybind11/src/Matrix3.hh +++ b/src/python_pybind11/src/Matrix3.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__MATRIX3_HH_ -#define IGNITION_MATH_PYTHON__MATRIX3_HH_ +#ifndef GZ_MATH_PYTHON__MATRIX3_HH_ +#define GZ_MATH_PYTHON__MATRIX3_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Matrix3 +/// Define a pybind11 wrapper for an gz::math::Matrix3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathMatrix3(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::Matrix3 +/// Help define a pybind11 wrapper for an gz::math::Matrix3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -49,7 +49,7 @@ void defineMathMatrix3(py::module &m, const std::string &typestr); template void helpDefineMathMatrix3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Matrix3; + using Class = gz::math::Matrix3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -63,12 +63,12 @@ void helpDefineMathMatrix3(py::module &m, const std::string &typestr) .def(py::init<>()) .def(py::init()) .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(py::self * float()) - .def(py::self * ignition::math::Vector3()) + .def(py::self * gz::math::Vector3()) // .def(py::self * py::self) // .def(py::self += py::self) // .def(-py::self) @@ -125,4 +125,4 @@ void helpDefineMathMatrix3(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__MATRIX3_HH_ +#endif // GZ_MATH_PYTHON__MATRIX3_HH_ diff --git a/src/python_pybind11/src/Matrix4.cc b/src/python_pybind11/src/Matrix4.cc index 42c6bae50..56c6974e5 100644 --- a/src/python_pybind11/src/Matrix4.cc +++ b/src/python_pybind11/src/Matrix4.cc @@ -33,5 +33,5 @@ void defineMathMatrix4(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Matrix4.hh b/src/python_pybind11/src/Matrix4.hh index 0782de3ad..0fda03b2d 100644 --- a/src/python_pybind11/src/Matrix4.hh +++ b/src/python_pybind11/src/Matrix4.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__MATRIX4_HH_ -#define IGNITION_MATH_PYTHON__MATRIX4_HH_ +#ifndef GZ_MATH_PYTHON__MATRIX4_HH_ +#define GZ_MATH_PYTHON__MATRIX4_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Matrix4 +/// Define a pybind11 wrapper for an gz::math::Matrix4 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathMatrix4(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::Matrix4 +/// Define a pybind11 wrapper for an gz::math::Matrix4 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -49,7 +49,7 @@ void defineMathMatrix4(py::module &m, const std::string &typestr); template void helpDefineMathMatrix4(py::module &m, const std::string &typestr) { - using Class = ignition::math::Matrix4; + using Class = gz::math::Matrix4; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -63,10 +63,10 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) .def(py::init<>()) .def(py::init()) .def(py::init()) - .def(py::init&>()) - .def(py::init&>()) + .def(py::init&>()) + .def(py::init&>()) .def(py::self * py::self) - .def(py::self * ignition::math::Vector3()) + .def(py::self * gz::math::Vector3()) .def(py::self == py::self) .def(py::self != py::self) .def("__call__", @@ -79,7 +79,7 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) py::overload_cast(&Class::SetTranslation), "Set the translational values [ (0, 3) (1, 3) (2, 3) ]") .def("set_translation", - py::overload_cast&>( + py::overload_cast&>( &Class::SetTranslation), "Set the translational values [ (0, 3) (1, 3) (2, 3) ]") .def("translation", @@ -110,7 +110,7 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) &Class::Pose, "Get the transformation as math::Pose") .def("scale", - py::overload_cast&>( + py::overload_cast&>( &Class::Scale), "Get the scale values as a Vector3") .def("scale", @@ -120,8 +120,8 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) &Class::IsAffine, "Get the scale values as a Vector3") .def("transform_affine", - py::overload_cast&, - ignition::math::Vector3&>( + py::overload_cast&, + gz::math::Vector3&>( &Class::TransformAffine, py::const_), "Perform an affine transformation") .def("equal", @@ -129,9 +129,9 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) "Equality test operator") .def("look_at", &Class::LookAt, - // py::arg("_eye") = ignition::math::Vector3::Zero, - py::arg("_target") = ignition::math::Vector3::Zero, - py::arg("_up") = ignition::math::Vector3::UnitZ, + // py::arg("_eye") = gz::math::Vector3::Zero, + py::arg("_target") = gz::math::Vector3::Zero, + py::arg("_up") = gz::math::Vector3::UnitZ, "Get transform which translates to _eye and rotates the X axis " "so it faces the _target. The rotation is such that Z axis is in the" "_up direction, if possible. The coordinate system is right-handed") @@ -147,7 +147,7 @@ void helpDefineMathMatrix4(py::module &m, const std::string &typestr) .def("__repr__", toString); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__MATRIX4_HH_ +#endif // GZ_MATH_PYTHON__MATRIX4_HH_ diff --git a/src/python_pybind11/src/Matrix6.hh b/src/python_pybind11/src/Matrix6.hh index 9a2e87786..06580ad7e 100644 --- a/src/python_pybind11/src/Matrix6.hh +++ b/src/python_pybind11/src/Matrix6.hh @@ -23,7 +23,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; diff --git a/src/python_pybind11/src/MovingWindowFilter.cc b/src/python_pybind11/src/MovingWindowFilter.cc index 264178e52..c27679c11 100644 --- a/src/python_pybind11/src/MovingWindowFilter.cc +++ b/src/python_pybind11/src/MovingWindowFilter.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "MovingWindowFilter.hh" namespace ignition @@ -30,9 +30,9 @@ void defineMathMovingWindowFilter(py::module &m, const std::string &typestr) { helpDefineMathMovingWindowFilter(m, typestr + "i"); helpDefineMathMovingWindowFilter(m, typestr + "d"); - helpDefineMathMovingWindowFilter(m, typestr + "v3"); + helpDefineMathMovingWindowFilter(m, typestr + "v3"); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/MovingWindowFilter.hh b/src/python_pybind11/src/MovingWindowFilter.hh index 0e4988899..68651766d 100644 --- a/src/python_pybind11/src/MovingWindowFilter.hh +++ b/src/python_pybind11/src/MovingWindowFilter.hh @@ -15,14 +15,14 @@ * */ -#ifndef IGNITION_MATH_PYTHON__MOVINGWINDOWFILTER_HH_ -#define IGNITION_MATH_PYTHON__MOVINGWINDOWFILTER_HH_ +#ifndef GZ_MATH_PYTHON__MOVINGWINDOWFILTER_HH_ +#define GZ_MATH_PYTHON__MOVINGWINDOWFILTER_HH_ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -33,7 +33,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::MovingWindowFilter +/// Help define a pybind11 wrapper for an gz::math::MovingWindowFilter /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -41,7 +41,7 @@ namespace python template void helpDefineMathMovingWindowFilter(py::module &m, const std::string &typestr) { - using Class = ignition::math::MovingWindowFilter; + using Class = gz::math::MovingWindowFilter; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -57,7 +57,7 @@ void helpDefineMathMovingWindowFilter(py::module &m, const std::string &typestr) .def("value", &Class::Value, "Get filtered result"); } -/// Define a pybind11 wrapper for an ignition::math::MovingWindowFilter +/// Define a pybind11 wrapper for an gz::math::MovingWindowFilter /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -67,4 +67,4 @@ void defineMathMovingWindowFilter(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__MovingWindowFilterD_HH_ +#endif // GZ_MATH_PYTHON__MovingWindowFilterD_HH_ diff --git a/src/python_pybind11/src/OrientedBox.hh b/src/python_pybind11/src/OrientedBox.hh index 37587380a..c416370bc 100644 --- a/src/python_pybind11/src/OrientedBox.hh +++ b/src/python_pybind11/src/OrientedBox.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ -#define IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ +#ifndef GZ_MATH_PYTHON__ORIENTEDBOX_HH_ +#define GZ_MATH_PYTHON__ORIENTEDBOX_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,7 +34,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::OrientedBox +/// Define a pybind11 wrapper for an gz::math::OrientedBox /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,7 +43,7 @@ template void defineMathOrientedBox(py::module &m, const std::string &typestr) { - using Class = ignition::math::OrientedBox; + using Class = gz::math::OrientedBox; std::string pyclass_name = typestr; auto toString = [](const Class &si) { std::stringstream stream; @@ -55,19 +55,19 @@ void defineMathOrientedBox(py::module &m, const std::string &typestr) py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init&, - const ignition::math::Pose3&>()) - .def(py::init&, - const ignition::math::Pose3&, - const ignition::math::Material&>()) - .def(py::init&>()) + .def(py::init&, + const gz::math::Pose3&>()) + .def(py::init&, + const gz::math::Pose3&, + const gz::math::Material&>()) + .def(py::init&>()) .def(py::init()) - .def(py::init&, - const ignition::math::Material&>()) + .def(py::init&, + const gz::math::Material&>()) .def(py::self != py::self) .def(py::self == py::self) .def("pose", - py::overload_cast&>(&Class::Pose), + py::overload_cast&>(&Class::Pose), "Set the box pose, which is the pose of its center.") .def("pose", py::overload_cast<>(&Class::Pose, py::const_), @@ -76,7 +76,7 @@ void defineMathOrientedBox(py::module &m, const std::string &typestr) py::overload_cast<>(&Class::Size, py::const_), "Get the size of the OrientedBox.") .def("size", - py::overload_cast&> + py::overload_cast&> (&Class::Size), "Set the size of the OrientedBox.") .def("x_length", @@ -125,4 +125,4 @@ void defineMathOrientedBox(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__ORIENTEDBOX_HH_ +#endif // GZ_MATH_PYTHON__ORIENTEDBOX_HH_ diff --git a/src/python_pybind11/src/PID.cc b/src/python_pybind11/src/PID.cc index fe53e00a0..9e273d1a6 100644 --- a/src/python_pybind11/src/PID.cc +++ b/src/python_pybind11/src/PID.cc @@ -18,7 +18,7 @@ #include #include -#include +#include #include "PID.hh" @@ -30,7 +30,7 @@ namespace python { void defineMathPID(py::module &m, const std::string &typestr) { - using Class = ignition::math::PID; + using Class = gz::math::PID; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/PID.hh b/src/python_pybind11/src/PID.hh index 819fba131..fe8791e5d 100644 --- a/src/python_pybind11/src/PID.hh +++ b/src/python_pybind11/src/PID.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__PID_HH_ -#define IGNITION_MATH_PYTHON__PID_HH_ +#ifndef GZ_MATH_PYTHON__PID_HH_ +#define GZ_MATH_PYTHON__PID_HH_ #include #include @@ -28,7 +28,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::PID +/// Define a pybind11 wrapper for an gz::math::PID /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -38,4 +38,4 @@ void defineMathPID(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__PID_HH_ +#endif // GZ_MATH_PYTHON__PID_HH_ diff --git a/src/python_pybind11/src/Plane.hh b/src/python_pybind11/src/Plane.hh index deadb6526..79e9d4c09 100644 --- a/src/python_pybind11/src/Plane.hh +++ b/src/python_pybind11/src/Plane.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__PLANE_HH_ -#define IGNITION_MATH_PYTHON__PLANE_HH_ +#ifndef GZ_MATH_PYTHON__PLANE_HH_ +#define GZ_MATH_PYTHON__PLANE_HH_ #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Plane +/// Define a pybind11 wrapper for an gz::math::Plane /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,52 +43,52 @@ namespace python template void defineMathPlane(py::module &m, const std::string &typestr) { - using Class = ignition::math::Plane; + using Class = gz::math::Plane; std::string pyclass_name = typestr; py::class_ plane(m, pyclass_name.c_str(), py::buffer_protocol(), py::dynamic_attr()); plane.def(py::init<>()) - .def(py::init&, T>(), - py::arg("_normal") = ignition::math::Vector3::Zero, + .def(py::init&, T>(), + py::arg("_normal") = gz::math::Vector3::Zero, py::arg("_offset") = 0.0) - .def(py::init&, - const ignition::math::Vector2&, T>()) + .def(py::init&, + const gz::math::Vector2&, T>()) .def(py::init()) .def("set", - py::overload_cast&, T> + py::overload_cast&, T> (&Class::Set), "Set the plane") .def("set", - py::overload_cast&, - const ignition::math::Vector2&, T> + py::overload_cast&, + const gz::math::Vector2&, T> (&Class::Set), "Set the plane") .def("distance", - py::overload_cast&> + py::overload_cast&> (&Class::Distance, py::const_), "The distance to the plane from the given point. The " "distance can be negative, which indicates the point is on the " "negative side of the plane.") .def("distance", - py::overload_cast&, - const ignition::math::Vector3&> + py::overload_cast&, + const gz::math::Vector3&> (&Class::Distance, py::const_), "Get distance to the plane give an origin and direction.") .def("intersection", &Class::Intersection, - py::arg("_point") = ignition::math::Vector3::Zero, - py::arg("_gradient") = ignition::math::Vector3::Zero, + py::arg("_point") = gz::math::Vector3::Zero, + py::arg("_gradient") = gz::math::Vector3::Zero, py::arg("_tolerance") = 1e-6, "Get the intersection of an infinite line with the plane, " "given the line's gradient and a point in parametrized space.") .def("side", - py::overload_cast&> + py::overload_cast&> (&Class::Side, py::const_), "The side of the plane a point is on.") .def("side", - py::overload_cast + py::overload_cast (&Class::Side, py::const_), "The side of the plane a point is on.") .def("size", @@ -109,11 +109,11 @@ void defineMathPlane(py::module &m, const std::string &typestr) return Class(self); }, "memo"_a); - py::enum_(m, "PlaneSide") - .value("NEGATIVE_SIDE", ignition::math::Planed::PlaneSide::NEGATIVE_SIDE) - .value("POSITIVE_SIDE", ignition::math::Planed::PlaneSide::POSITIVE_SIDE) - .value("NO_SIDE", ignition::math::Planed::PlaneSide::NO_SIDE) - .value("BOTH_SIDE", ignition::math::Planed::PlaneSide::BOTH_SIDE) + py::enum_(m, "PlaneSide") + .value("NEGATIVE_SIDE", gz::math::Planed::PlaneSide::NEGATIVE_SIDE) + .value("POSITIVE_SIDE", gz::math::Planed::PlaneSide::POSITIVE_SIDE) + .value("NO_SIDE", gz::math::Planed::PlaneSide::NO_SIDE) + .value("BOTH_SIDE", gz::math::Planed::PlaneSide::BOTH_SIDE) .export_values(); } @@ -121,4 +121,4 @@ void defineMathPlane(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__PLANE_HH_ +#endif // GZ_MATH_PYTHON__PLANE_HH_ diff --git a/src/python_pybind11/src/Polynomial3.hh b/src/python_pybind11/src/Polynomial3.hh index 7b0fede59..27dec56c7 100644 --- a/src/python_pybind11/src/Polynomial3.hh +++ b/src/python_pybind11/src/Polynomial3.hh @@ -15,16 +15,16 @@ * */ -#ifndef IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ -#define IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ +#ifndef GZ_MATH_PYTHON__POLYNOMIAL3_HH_ +#define GZ_MATH_PYTHON__POLYNOMIAL3_HH_ #include #include #include -#include -#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Polynomial3 +/// Help define a pybind11 wrapper for an gz::math::Polynomial3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -43,7 +43,7 @@ namespace python template void helpDefineMathPolynomial3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Polynomial3; + using Class = gz::math::Polynomial3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -97,7 +97,7 @@ void helpDefineMathPolynomial3(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Polynomial3 +/// Define a pybind11 wrapper for an gz::math::Polynomial3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -108,4 +108,4 @@ void defineMathPolynomial3(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ +#endif // GZ_MATH_PYTHON__POLYNOMIAL3_HH_ diff --git a/src/python_pybind11/src/Pose3.cc b/src/python_pybind11/src/Pose3.cc index b79de9d8e..2a9e1d117 100644 --- a/src/python_pybind11/src/Pose3.cc +++ b/src/python_pybind11/src/Pose3.cc @@ -33,5 +33,5 @@ void defineMathPose3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Pose3.hh b/src/python_pybind11/src/Pose3.hh index 16635b9aa..0441ce104 100644 --- a/src/python_pybind11/src/Pose3.hh +++ b/src/python_pybind11/src/Pose3.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__POSE3_HH_ -#define IGNITION_MATH_PYTHON__POSE3_HH_ +#ifndef GZ_MATH_PYTHON__POSE3_HH_ +#define GZ_MATH_PYTHON__POSE3_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Pose3 +/// Define a pybind11 wrapper for an gz::math::Pose3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathPose3(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::Pose3 +/// Help define a pybind11 wrapper for an gz::math::Pose3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -49,7 +49,7 @@ void defineMathPose3(py::module &m, const std::string &typestr); template void helpDefineMathPose3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Pose3; + using Class = gz::math::Pose3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -61,8 +61,8 @@ void helpDefineMathPose3(py::module &m, const std::string &typestr) py::buffer_protocol(), py::dynamic_attr()) .def(py::init<>()) - .def(py::init&, - const ignition::math::Quaternion&>()) + .def(py::init&, + const gz::math::Quaternion&>()) .def(py::init()) .def(py::init()) .def(py::init()) @@ -76,12 +76,12 @@ void helpDefineMathPose3(py::module &m, const std::string &typestr) .def(py::self * py::self) .def(py::self *= py::self) .def("set", - py::overload_cast&, - const ignition::math::Quaternion&>(&Class::Set), + py::overload_cast&, + const gz::math::Quaternion&>(&Class::Set), "Set the pose from a Vector3 and a Quaternion") .def("set", - py::overload_cast&, - const ignition::math::Vector3&>(&Class::Set), + py::overload_cast&, + const gz::math::Vector3&>(&Class::Set), "Set the pose from pos and rpy vectors") .def("set", py::overload_cast(&Class::Set), @@ -96,7 +96,7 @@ void helpDefineMathPose3(py::module &m, const std::string &typestr) &Class::Inverse, "Get the inverse of this pose") .def("coord_position_add", - py::overload_cast&>( + py::overload_cast&>( &Class::CoordPositionAdd, py::const_), "Add one point to a vector: result = this + pos") .def("coord_position_add", @@ -151,7 +151,7 @@ void helpDefineMathPose3(py::module &m, const std::string &typestr) .def("__repr__", toString); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__POSE3_HH_ +#endif // GZ_MATH_PYTHON__POSE3_HH_ diff --git a/src/python_pybind11/src/Quaternion.cc b/src/python_pybind11/src/Quaternion.cc index c6cf443e6..3f05f53ef 100644 --- a/src/python_pybind11/src/Quaternion.cc +++ b/src/python_pybind11/src/Quaternion.cc @@ -33,5 +33,5 @@ void defineMathQuaternion(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Quaternion.hh b/src/python_pybind11/src/Quaternion.hh index 5f5b96ac1..7d78882b3 100644 --- a/src/python_pybind11/src/Quaternion.hh +++ b/src/python_pybind11/src/Quaternion.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__QUATERNION_HH_ -#define IGNITION_MATH_PYTHON__QUATERNION_HH_ +#ifndef GZ_MATH_PYTHON__QUATERNION_HH_ +#define GZ_MATH_PYTHON__QUATERNION_HH_ #include @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -37,14 +37,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Quaternion +/// Define a pybind11 wrapper for an gz::math::Quaternion /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathQuaternion(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::Quaternion +/// Help define a pybind11 wrapper for an gz::math::Quaternion /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -52,7 +52,7 @@ void defineMathQuaternion(py::module &m, const std::string &typestr); template void helpDefineMathQuaternion(py::module &m, const std::string &typestr) { - using Class = ignition::math::Quaternion; + using Class = gz::math::Quaternion; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -66,9 +66,9 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) .def(py::init<>()) .def(py::init()) .def(py::init()) - .def(py::init&, T>()) - .def(py::init&>()) - .def(py::init&>()) + .def(py::init&, T>()) + .def(py::init&>()) + .def(py::init&>()) .def(py::init()) .def(py::self + py::self) .def(py::self += py::self) @@ -77,7 +77,7 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) .def(py::self -= py::self) .def(py::self * py::self) .def(py::self * float()) - .def(py::self * ignition::math::Vector3()) + .def(py::self * gz::math::Vector3()) .def(py::self *= py::self) .def(py::self == py::self) .def(py::self != py::self) @@ -105,13 +105,13 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) py::overload_cast(&Class::Axis), "Set the quaternion from an axis and angle") .def("axis", - py::overload_cast&, T>(&Class::Axis), + py::overload_cast&, T>(&Class::Axis), "Set the quaternion from an axis and angle") .def("set", &Class::Set, "Set this quaternion from 4 floating numbers") .def("euler", - py::overload_cast&>(&Class::Euler), + py::overload_cast&>(&Class::Euler), "Set the quaternion from Euler angles. The order of operations " "is roll, pitch, yaw around a fixed body frame axis " "(the original frame of the object before rotation is applied).") @@ -122,7 +122,7 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) py::overload_cast<>(&Class::Euler, py::const_), "Return the rotation in Euler angles") .def("euler_to_quaternion", - py::overload_cast&>( + py::overload_cast&>( &Class::EulerToQuaternion), "Convert euler angles to quatern.") .def("euler_to_quaternion", @@ -133,7 +133,7 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) .def("yaw", &Class::Yaw, "Get the Euler yaw angle in radians") .def("to_axis", [](const Class &self) { - ignition::math::Vector3 _axis; + gz::math::Vector3 _axis; T _angle; self.ToAxis(_axis, _angle); return std::make_tuple(_axis, _angle); @@ -214,4 +214,4 @@ void helpDefineMathQuaternion(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__QUATERNION_HH_ +#endif // GZ_MATH_PYTHON__QUATERNION_HH_ diff --git a/src/python_pybind11/src/Rand.cc b/src/python_pybind11/src/Rand.cc index eae9181e6..b9aff9cc8 100644 --- a/src/python_pybind11/src/Rand.cc +++ b/src/python_pybind11/src/Rand.cc @@ -17,7 +17,7 @@ #include #include "Rand.hh" -#include +#include namespace ignition { @@ -27,7 +27,7 @@ namespace python { void defineMathRand(py::module &m, const std::string &typestr) { - using Class = ignition::math::Rand; + using Class = gz::math::Rand; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -35,24 +35,24 @@ void defineMathRand(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def("seed", - py::overload_cast<>(&ignition::math::Rand::Seed), + py::overload_cast<>(&gz::math::Rand::Seed), "Get the seed value.") .def("seed", - py::overload_cast(&ignition::math::Rand::Seed), + py::overload_cast(&gz::math::Rand::Seed), "Set the seed value.") .def("dbl_uniform", - ignition::math::Rand::DblUniform, + gz::math::Rand::DblUniform, "Get a double from a uniform distribution") .def("dbl_normal", - ignition::math::Rand::DblNormal, + gz::math::Rand::DblNormal, "Get a double from a normal distribution") .def("int_uniform", - ignition::math::Rand::IntUniform, + gz::math::Rand::IntUniform, "Get a integer from a uniform distribution") .def("int_normal", - ignition::math::Rand::IntNormal, + gz::math::Rand::IntNormal, "Get a integer from a normal distribution"); } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Rand.hh b/src/python_pybind11/src/Rand.hh index 9242fdbf6..231d6275c 100644 --- a/src/python_pybind11/src/Rand.hh +++ b/src/python_pybind11/src/Rand.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__RAND_HH_ -#define IGNITION_MATH_PYTHON__RAND_HH_ +#ifndef GZ_MATH_PYTHON__RAND_HH_ +#define GZ_MATH_PYTHON__RAND_HH_ #include #include @@ -29,13 +29,13 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Rand +/// Define a pybind11 wrapper for an gz::math::Rand /** * \param[in] module a pybind11 module to add the definition to */ void defineMathRand(py::module &m, const std::string &typestr); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__RAND_HH_ +#endif // GZ_MATH_PYTHON__RAND_HH_ diff --git a/src/python_pybind11/src/Region3.hh b/src/python_pybind11/src/Region3.hh index 9ca4cce73..90234cc31 100644 --- a/src/python_pybind11/src/Region3.hh +++ b/src/python_pybind11/src/Region3.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__REGION3_HH_ -#define IGNITION_MATH_PYTHON__REGION3_HH_ +#ifndef GZ_MATH_PYTHON__REGION3_HH_ +#define GZ_MATH_PYTHON__REGION3_HH_ #include #include @@ -24,8 +24,8 @@ #include #include -#include -#include +#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -36,7 +36,7 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Region3 +/// Help define a pybind11 wrapper for an gz::math::Region3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -44,7 +44,7 @@ namespace python template void helpDefineMathRegion3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Region3; + using Class = gz::math::Region3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -96,7 +96,7 @@ void helpDefineMathRegion3(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Region3 +/// Define a pybind11 wrapper for an gz::math::Region3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -107,4 +107,4 @@ void defineMathRegion3(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__REGION3_HH_ +#endif // GZ_MATH_PYTHON__REGION3_HH_ diff --git a/src/python_pybind11/src/RollingMean.cc b/src/python_pybind11/src/RollingMean.cc index c84279451..7719188b0 100644 --- a/src/python_pybind11/src/RollingMean.cc +++ b/src/python_pybind11/src/RollingMean.cc @@ -16,7 +16,7 @@ */ #include #include "RollingMean.hh" -#include +#include namespace ignition { @@ -26,7 +26,7 @@ namespace python { void defineMathRollingMean(py::module &m, const std::string &typestr) { - using Class = ignition::math::RollingMean; + using Class = gz::math::RollingMean; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/RollingMean.hh b/src/python_pybind11/src/RollingMean.hh index 122e513fc..44f9b8d39 100644 --- a/src/python_pybind11/src/RollingMean.hh +++ b/src/python_pybind11/src/RollingMean.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__ROLLINGMEAN_HH_ -#define IGNITION_MATH_PYTHON__ROLLINGMEAN_HH_ +#ifndef GZ_MATH_PYTHON__ROLLINGMEAN_HH_ +#define GZ_MATH_PYTHON__ROLLINGMEAN_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::RollingMean +/// Define a pybind11 wrapper for an gz::math::RollingMean /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathRollingMean(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__ROLLINGMEAN_HH_ +#endif // GZ_MATH_PYTHON__ROLLINGMEAN_HH_ diff --git a/src/python_pybind11/src/RotationSpline.cc b/src/python_pybind11/src/RotationSpline.cc index f457a9ba3..8413992d0 100644 --- a/src/python_pybind11/src/RotationSpline.cc +++ b/src/python_pybind11/src/RotationSpline.cc @@ -16,7 +16,7 @@ */ #include #include "RotationSpline.hh" -#include +#include namespace ignition { @@ -26,7 +26,7 @@ namespace python { void defineMathRotationSpline(py::module &m, const std::string &typestr) { - using Class = ignition::math::RotationSpline; + using Class = gz::math::RotationSpline; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/RotationSpline.hh b/src/python_pybind11/src/RotationSpline.hh index 670b2ad97..8814c5521 100644 --- a/src/python_pybind11/src/RotationSpline.hh +++ b/src/python_pybind11/src/RotationSpline.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__ROTATIONSPLINE_HH_ -#define IGNITION_MATH_PYTHON__ROTATIONSPLINE_HH_ +#ifndef GZ_MATH_PYTHON__ROTATIONSPLINE_HH_ +#define GZ_MATH_PYTHON__ROTATIONSPLINE_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::RotationSpline +/// Define a pybind11 wrapper for an gz::math::RotationSpline /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathRotationSpline(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__ROTATIONSPLINE_HH_ +#endif // GZ_MATH_PYTHON__ROTATIONSPLINE_HH_ diff --git a/src/python_pybind11/src/SemanticVersion.cc b/src/python_pybind11/src/SemanticVersion.cc index 3b8280fd5..6c4e30a23 100644 --- a/src/python_pybind11/src/SemanticVersion.cc +++ b/src/python_pybind11/src/SemanticVersion.cc @@ -17,7 +17,7 @@ #include -#include +#include #include #include "SemanticVersion.hh" @@ -32,7 +32,7 @@ namespace python { void defineMathSemanticVersion(py::module &m, const std::string &typestr) { - using Class = ignition::math::SemanticVersion; + using Class = gz::math::SemanticVersion; std::string pyclass_name = typestr; auto toString = [](const Class &si) { std::stringstream stream; diff --git a/src/python_pybind11/src/SemanticVersion.hh b/src/python_pybind11/src/SemanticVersion.hh index 0c600c2a3..8fede5de3 100644 --- a/src/python_pybind11/src/SemanticVersion.hh +++ b/src/python_pybind11/src/SemanticVersion.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__SEMANTICVERSION_HH_ -#define IGNITION_MATH_PYTHON__SEMANTICVERSION_HH_ +#ifndef GZ_MATH_PYTHON__SEMANTICVERSION_HH_ +#define GZ_MATH_PYTHON__SEMANTICVERSION_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::SemanticVersion +/// Define a pybind11 wrapper for an gz::math::SemanticVersion /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathSemanticVersion(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__SEMANTICVERSION_HH_ +#endif // GZ_MATH_PYTHON__SEMANTICVERSION_HH_ diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc index 69b31e661..afc7f07dd 100644 --- a/src/python_pybind11/src/SignalStats.cc +++ b/src/python_pybind11/src/SignalStats.cc @@ -20,7 +20,7 @@ #include #include "SignalStats.hh" -#include +#include namespace ignition { @@ -31,7 +31,7 @@ namespace python ////////////////////////////////////////////////// void defineMathSignalStats(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalStats; + using Class = gz::math::SignalStats; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -56,7 +56,7 @@ void defineMathSignalStats(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalStatisticTrampoline : public ignition::math::SignalStatistic +class SignalStatisticTrampoline : public gz::math::SignalStatistic { public: SignalStatisticTrampoline() : SignalStatistic() {} @@ -68,7 +68,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -78,7 +78,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -88,7 +88,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { PYBIND11_OVERLOAD_PURE( size_t, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Count, ); @@ -98,7 +98,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -109,7 +109,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Reset, ); @@ -119,7 +119,7 @@ class SignalStatisticTrampoline : public ignition::math::SignalStatistic ////////////////////////////////////////////////// void defineMathSignalStatistic(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalStatistic; + using Class = gz::math::SignalStatistic; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -145,7 +145,7 @@ void defineMathSignalStatistic(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalVarianceTrampoline : public ignition::math::SignalVariance { +class SignalVarianceTrampoline : public gz::math::SignalVariance { public: // Inherit the constructors SignalVarianceTrampoline(){} @@ -155,7 +155,7 @@ class SignalVarianceTrampoline : public ignition::math::SignalVariance { { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -165,7 +165,7 @@ class SignalVarianceTrampoline : public ignition::math::SignalVariance { { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -175,7 +175,7 @@ class SignalVarianceTrampoline : public ignition::math::SignalVariance { { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -186,7 +186,7 @@ class SignalVarianceTrampoline : public ignition::math::SignalVariance { ////////////////////////////////////////////////// void defineMathSignalVariance(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalVariance; + using Class = gz::math::SignalVariance; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -211,7 +211,7 @@ void defineMathSignalVariance(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalMaximumTrampoline : public ignition::math::SignalMaximum +class SignalMaximumTrampoline : public gz::math::SignalMaximum { public: // Inherit the constructors @@ -222,7 +222,7 @@ class SignalMaximumTrampoline : public ignition::math::SignalMaximum { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -232,7 +232,7 @@ class SignalMaximumTrampoline : public ignition::math::SignalMaximum { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -242,7 +242,7 @@ class SignalMaximumTrampoline : public ignition::math::SignalMaximum { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -253,7 +253,7 @@ class SignalMaximumTrampoline : public ignition::math::SignalMaximum ////////////////////////////////////////////////// void defineMathSignalMaximum(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalMaximum; + using Class = gz::math::SignalMaximum; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -278,7 +278,7 @@ void defineMathSignalMaximum(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalMinimumTrampoline : public ignition::math::SignalMinimum +class SignalMinimumTrampoline : public gz::math::SignalMinimum { public: // Inherit the constructors @@ -289,7 +289,7 @@ class SignalMinimumTrampoline : public ignition::math::SignalMinimum { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -299,7 +299,7 @@ class SignalMinimumTrampoline : public ignition::math::SignalMinimum { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -309,7 +309,7 @@ class SignalMinimumTrampoline : public ignition::math::SignalMinimum { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -320,7 +320,7 @@ class SignalMinimumTrampoline : public ignition::math::SignalMinimum ////////////////////////////////////////////////// void defineMathSignalMinimum(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalMinimum; + using Class = gz::math::SignalMinimum; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -345,7 +345,7 @@ void defineMathSignalMinimum(py::module &m, const std::string &typestr) } ////////////////////////////////////////////////// -class SignalMeanTrampoline : public ignition::math::SignalMean +class SignalMeanTrampoline : public gz::math::SignalMean { public: // Inherit the constructors @@ -356,7 +356,7 @@ class SignalMeanTrampoline : public ignition::math::SignalMean { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -366,7 +366,7 @@ class SignalMeanTrampoline : public ignition::math::SignalMean { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -376,7 +376,7 @@ class SignalMeanTrampoline : public ignition::math::SignalMean { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -387,7 +387,7 @@ class SignalMeanTrampoline : public ignition::math::SignalMean ////////////////////////////////////////////////// void defineMathSignalMean(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalMean; + using Class = gz::math::SignalMean; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -413,7 +413,7 @@ void defineMathSignalMean(py::module &m, const std::string &typestr) ////////////////////////////////////////////////// class SignalRootMeanSquareTrampoline : - public ignition::math::SignalRootMeanSquare + public gz::math::SignalRootMeanSquare { public: // Inherit the constructors @@ -424,7 +424,7 @@ class SignalRootMeanSquareTrampoline : { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -434,7 +434,7 @@ class SignalRootMeanSquareTrampoline : { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName ); @@ -444,7 +444,7 @@ class SignalRootMeanSquareTrampoline : { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -455,7 +455,7 @@ class SignalRootMeanSquareTrampoline : ////////////////////////////////////////////////// void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalRootMeanSquare; + using Class = gz::math::SignalRootMeanSquare; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -481,7 +481,7 @@ void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr) ////////////////////////////////////////////////// class SignalMaxAbsoluteValueTrampoline : - public ignition::math::SignalMaxAbsoluteValue + public gz::math::SignalMaxAbsoluteValue { public: // Inherit the constructors @@ -492,7 +492,7 @@ class SignalMaxAbsoluteValueTrampoline : { PYBIND11_OVERLOAD_PURE( double, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) Value, ); @@ -502,7 +502,7 @@ class SignalMaxAbsoluteValueTrampoline : { PYBIND11_OVERLOAD_PURE( std::string, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) ShortName, ); @@ -512,7 +512,7 @@ class SignalMaxAbsoluteValueTrampoline : { PYBIND11_OVERLOAD_PURE( void, // Return type (ret_type) - ignition::math::SignalStatistic, // Parent class (cname) + gz::math::SignalStatistic, // Parent class (cname) // Name of function in C++ (must match Python name) (fn) InsertData, _data // Argument(s) (...) @@ -523,7 +523,7 @@ class SignalMaxAbsoluteValueTrampoline : ////////////////////////////////////////////////// void defineMathSignalMaxAbsoluteValue(py::module &m, const std::string &typestr) { - using Class = ignition::math::SignalMaxAbsoluteValue; + using Class = gz::math::SignalMaxAbsoluteValue; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/SignalStats.hh b/src/python_pybind11/src/SignalStats.hh index 7e92e1940..fac514b64 100644 --- a/src/python_pybind11/src/SignalStats.hh +++ b/src/python_pybind11/src/SignalStats.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ -#define IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ +#ifndef GZ_MATH_PYTHON__SIGNALSTATS_HH_ +#define GZ_MATH_PYTHON__SIGNALSTATS_HH_ #include #include @@ -29,42 +29,42 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::SignalStats +/// Define a pybind11 wrapper for an gz::math::SignalStats /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalStats(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalStatistic +/// Define a pybind11 wrapper for an gz::math::SignalStatistic /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalStatistic(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalMaximum +/// Define a pybind11 wrapper for an gz::math::SignalMaximum /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalMaximum(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalMinimum +/// Define a pybind11 wrapper for an gz::math::SignalMinimum /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalMinimum(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalVariance +/// Define a pybind11 wrapper for an gz::math::SignalVariance /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalVariance(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalMaxAbsoluteValue +/// Define a pybind11 wrapper for an gz::math::SignalMaxAbsoluteValue /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -72,14 +72,14 @@ void defineMathSignalVariance(py::module &m, const std::string &typestr); void defineMathSignalMaxAbsoluteValue( py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalRootMeanSquare +/// Define a pybind11 wrapper for an gz::math::SignalRootMeanSquare /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr); -/// Define a pybind11 wrapper for an ignition::math::SignalMean +/// Define a pybind11 wrapper for an gz::math::SignalMean /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -89,4 +89,4 @@ void defineMathSignalMean(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ +#endif // GZ_MATH_PYTHON__SIGNALSTATS_HH_ diff --git a/src/python_pybind11/src/Sphere.hh b/src/python_pybind11/src/Sphere.hh index 4e005e80c..12c1bc638 100644 --- a/src/python_pybind11/src/Sphere.hh +++ b/src/python_pybind11/src/Sphere.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__SPHERE_HH_ -#define IGNITION_MATH_PYTHON__SPHERE_HH_ +#ifndef GZ_MATH_PYTHON__SPHERE_HH_ +#define GZ_MATH_PYTHON__SPHERE_HH_ #include #include @@ -24,7 +24,7 @@ #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -35,7 +35,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Sphere +/// Define a pybind11 wrapper for an gz::math::Sphere /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -44,7 +44,7 @@ template void defineMathSphere(py::module &m, const std::string &typestr) { - using Class = ignition::math::Sphere; + using Class = gz::math::Sphere; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), @@ -52,7 +52,7 @@ void defineMathSphere(py::module &m, const std::string &typestr) py::dynamic_attr()) .def(py::init<>()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def(py::self != py::self) .def(py::self == py::self) .def("radius", @@ -100,4 +100,4 @@ void defineMathSphere(py::module &m, const std::string &typestr) } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__SPHERE_HH_ +#endif // GZ_MATH_PYTHON__SPHERE_HH_ diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc index 9bf0dee7c..442587243 100644 --- a/src/python_pybind11/src/SphericalCoordinates.cc +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -17,8 +17,8 @@ #include #include "SphericalCoordinates.hh" -#include -#include +#include +#include namespace ignition { @@ -28,7 +28,7 @@ namespace python { void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) { - using Class = ignition::math::SphericalCoordinates; + using Class = gz::math::SphericalCoordinates; std::string pyclass_name = typestr; py::class_ sphericalCoordinates(m, @@ -39,9 +39,9 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) .def(py::init<>()) .def(py::init()) .def(py::init()) - .def(py::init()) + .def(py::init()) .def(py::self != py::self) .def(py::self == py::self) .def("spherical_from_local_position", diff --git a/src/python_pybind11/src/SphericalCoordinates.hh b/src/python_pybind11/src/SphericalCoordinates.hh index e7a2b83cd..927dfe060 100644 --- a/src/python_pybind11/src/SphericalCoordinates.hh +++ b/src/python_pybind11/src/SphericalCoordinates.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ -#define IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ +#ifndef GZ_MATH_PYTHON__SPHERICALCOORDINATES_HH_ +#define GZ_MATH_PYTHON__SPHERICALCOORDINATES_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::SphericalCoordinates +/// Define a pybind11 wrapper for an gz::math::SphericalCoordinates /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ +#endif // GZ_MATH_PYTHON__SPHERICALCOORDINATES_HH_ diff --git a/src/python_pybind11/src/Spline.cc b/src/python_pybind11/src/Spline.cc index c5ce95ecc..be0fd1723 100644 --- a/src/python_pybind11/src/Spline.cc +++ b/src/python_pybind11/src/Spline.cc @@ -16,7 +16,7 @@ */ #include #include "Spline.hh" -#include +#include namespace ignition { @@ -26,7 +26,7 @@ namespace python { void defineMathSpline(py::module &m, const std::string &typestr) { - using Class = ignition::math::Spline; + using Class = gz::math::Spline; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/Spline.hh b/src/python_pybind11/src/Spline.hh index b0fbd17d8..099a39a67 100644 --- a/src/python_pybind11/src/Spline.hh +++ b/src/python_pybind11/src/Spline.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__SPLINE_HH_ -#define IGNITION_MATH_PYTHON__SPLINE_HH_ +#ifndef GZ_MATH_PYTHON__SPLINE_HH_ +#define GZ_MATH_PYTHON__SPLINE_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::RollingMean +/// Define a pybind11 wrapper for an gz::math::RollingMean /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathSpline(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__SPLINE_HH_ +#endif // GZ_MATH_PYTHON__SPLINE_HH_ diff --git a/src/python_pybind11/src/StopWatch.cc b/src/python_pybind11/src/StopWatch.cc index 2d2dbe5cf..302c8cd39 100644 --- a/src/python_pybind11/src/StopWatch.cc +++ b/src/python_pybind11/src/StopWatch.cc @@ -21,7 +21,7 @@ #include "StopWatch.hh" -#include +#include namespace ignition { @@ -31,7 +31,7 @@ namespace python { void defineMathStopwatch(py::module &m, const std::string &typestr) { - using Class = ignition::math::Stopwatch; + using Class = gz::math::Stopwatch; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/StopWatch.hh b/src/python_pybind11/src/StopWatch.hh index 672ee00f1..c62e8fce3 100644 --- a/src/python_pybind11/src/StopWatch.hh +++ b/src/python_pybind11/src/StopWatch.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__STOPWATCH_HH_ -#define IGNITION_MATH_PYTHON__STOPWATCH_HH_ +#ifndef GZ_MATH_PYTHON__STOPWATCH_HH_ +#define GZ_MATH_PYTHON__STOPWATCH_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Stopwatch +/// Define a pybind11 wrapper for an gz::math::Stopwatch /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathStopwatch(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__STOPWATCH_HH_ +#endif // GZ_MATH_PYTHON__STOPWATCH_HH_ diff --git a/src/python_pybind11/src/Temperature.cc b/src/python_pybind11/src/Temperature.cc index 32c260b42..c3de5ecb2 100644 --- a/src/python_pybind11/src/Temperature.cc +++ b/src/python_pybind11/src/Temperature.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "Temperature.hh" #include @@ -32,7 +32,7 @@ namespace python { void defineMathTemperature(py::module &m, const std::string &typestr) { - using Class = ignition::math::Temperature; + using Class = gz::math::Temperature; std::string pyclass_name = typestr; auto toString = [](const Class &si) { std::stringstream stream; diff --git a/src/python_pybind11/src/Temperature.hh b/src/python_pybind11/src/Temperature.hh index 9307a1f05..2624bed25 100644 --- a/src/python_pybind11/src/Temperature.hh +++ b/src/python_pybind11/src/Temperature.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__TEMPERATURE_HH_ -#define IGNITION_MATH_PYTHON__TEMPERATURE_HH_ +#ifndef GZ_MATH_PYTHON__TEMPERATURE_HH_ +#define GZ_MATH_PYTHON__TEMPERATURE_HH_ #include #include @@ -29,7 +29,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Temperature +/// Define a pybind11 wrapper for an gz::math::Temperature /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -39,4 +39,4 @@ void defineMathTemperature(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__TEMPERATURE_HH_ +#endif // GZ_MATH_PYTHON__TEMPERATURE_HH_ diff --git a/src/python_pybind11/src/Triangle.cc b/src/python_pybind11/src/Triangle.cc index 12a843218..ffe303e42 100644 --- a/src/python_pybind11/src/Triangle.cc +++ b/src/python_pybind11/src/Triangle.cc @@ -33,5 +33,5 @@ void defineMathTriangle(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Triangle.hh b/src/python_pybind11/src/Triangle.hh index 407ee207f..8366c1283 100644 --- a/src/python_pybind11/src/Triangle.hh +++ b/src/python_pybind11/src/Triangle.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__TRIANGLE_HH_ -#define IGNITION_MATH_PYTHON__TRIANGLE_HH_ +#ifndef GZ_MATH_PYTHON__TRIANGLE_HH_ +#define GZ_MATH_PYTHON__TRIANGLE_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Triangle +/// Define a pybind11 wrapper for an gz::math::Triangle /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathTriangle(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::Triangle +/// Help define a pybind11 wrapper for an gz::math::Triangle /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -49,7 +49,7 @@ void defineMathTriangle(py::module &m, const std::string &typestr); template void helpDefineMathTriangle(py::module &m, const std::string &typestr) { - using Class = ignition::math::Triangle; + using Class = gz::math::Triangle; py::class_(m, typestr.c_str(), py::buffer_protocol(), @@ -106,7 +106,7 @@ void helpDefineMathTriangle(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__TRIANGLE_HH_ +#endif // GZ_MATH_PYTHON__TRIANGLE_HH_ diff --git a/src/python_pybind11/src/Triangle3.cc b/src/python_pybind11/src/Triangle3.cc index 52083b61a..8e3de24e1 100644 --- a/src/python_pybind11/src/Triangle3.cc +++ b/src/python_pybind11/src/Triangle3.cc @@ -33,5 +33,5 @@ void defineMathTriangle3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Triangle3.hh b/src/python_pybind11/src/Triangle3.hh index 9d0a8b223..a076bbceb 100644 --- a/src/python_pybind11/src/Triangle3.hh +++ b/src/python_pybind11/src/Triangle3.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__TRIANGLE3_HH_ -#define IGNITION_MATH_PYTHON__TRIANGLE3_HH_ +#ifndef GZ_MATH_PYTHON__TRIANGLE3_HH_ +#define GZ_MATH_PYTHON__TRIANGLE3_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Triangle3 +/// Define a pybind11 wrapper for an gz::math::Triangle3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python */ void defineMathTriangle3(py::module &m, const std::string &typestr); -/// Help define a pybind11 wrapper for an ignition::math::Triangle3 +/// Help define a pybind11 wrapper for an gz::math::Triangle3 /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -49,7 +49,7 @@ void defineMathTriangle3(py::module &m, const std::string &typestr); template void helpDefineMathTriangle3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Triangle3; + using Class = gz::math::Triangle3; py::class_(m, typestr.c_str(), py::buffer_protocol(), @@ -107,7 +107,7 @@ void helpDefineMathTriangle3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__TRIANGLE3_HH_ +#endif // GZ_MATH_PYTHON__TRIANGLE3_HH_ diff --git a/src/python_pybind11/src/Vector2.cc b/src/python_pybind11/src/Vector2.cc index 09de45755..1e135c352 100644 --- a/src/python_pybind11/src/Vector2.cc +++ b/src/python_pybind11/src/Vector2.cc @@ -33,5 +33,5 @@ void defineMathVector2(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Vector2.hh b/src/python_pybind11/src/Vector2.hh index 171980d88..ea1076db9 100644 --- a/src/python_pybind11/src/Vector2.hh +++ b/src/python_pybind11/src/Vector2.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__VECTOR2_HH_ -#define IGNITION_MATH_PYTHON__VECTOR2_HH_ +#ifndef GZ_MATH_PYTHON__VECTOR2_HH_ +#define GZ_MATH_PYTHON__VECTOR2_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Vector2 +/// Help define a pybind11 wrapper for an gz::math::Vector2 /** * \param[in] module a pybind11 module to add the definition to */ template void helpDefineMathVector2(py::module &m, const std::string &typestr) { - using Class = ignition::math::Vector2; + using Class = gz::math::Vector2; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -136,13 +136,13 @@ void helpDefineMathVector2(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Vector2 +/// Define a pybind11 wrapper for an gz::math::Vector2 /** * \param[in] module a pybind11 module to add the definition to */ void defineMathVector2(py::module &m, const std::string &typestr); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__VECTOR2_HH_ +#endif // GZ_MATH_PYTHON__VECTOR2_HH_ diff --git a/src/python_pybind11/src/Vector3.cc b/src/python_pybind11/src/Vector3.cc index a38f0e07a..64a312c0e 100644 --- a/src/python_pybind11/src/Vector3.cc +++ b/src/python_pybind11/src/Vector3.cc @@ -33,5 +33,5 @@ void defineMathVector3(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Vector3.hh b/src/python_pybind11/src/Vector3.hh index a97fc3417..5d55c524c 100644 --- a/src/python_pybind11/src/Vector3.hh +++ b/src/python_pybind11/src/Vector3.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__VECTOR3_HH_ -#define IGNITION_MATH_PYTHON__VECTOR3_HH_ +#ifndef GZ_MATH_PYTHON__VECTOR3_HH_ +#define GZ_MATH_PYTHON__VECTOR3_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Vector3 +/// Help define a pybind11 wrapper for an gz::math::Vector3 /** * \param[in] module a pybind11 module to add the definition to */ template void helpDefineMathVector3(py::module &m, const std::string &typestr) { - using Class = ignition::math::Vector3; + using Class = gz::math::Vector3; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -160,13 +160,13 @@ void helpDefineMathVector3(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Vector2 +/// Define a pybind11 wrapper for an gz::math::Vector2 /** * \param[in] module a pybind11 module to add the definition to */ void defineMathVector3(py::module &m, const std::string &typestr); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__VECTOR3_HH_ +#endif // GZ_MATH_PYTHON__VECTOR3_HH_ diff --git a/src/python_pybind11/src/Vector3Stats.cc b/src/python_pybind11/src/Vector3Stats.cc index a0081819c..7df3426e1 100644 --- a/src/python_pybind11/src/Vector3Stats.cc +++ b/src/python_pybind11/src/Vector3Stats.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "Vector3Stats.hh" @@ -31,7 +31,7 @@ namespace python { void defineMathVector3Stats(py::module &m, const std::string &typestr) { - using Class = ignition::math::Vector3Stats; + using Class = gz::math::Vector3Stats; std::string pyclass_name = typestr; py::class_(m, pyclass_name.c_str(), diff --git a/src/python_pybind11/src/Vector3Stats.hh b/src/python_pybind11/src/Vector3Stats.hh index bd1848ac3..fb8580c41 100644 --- a/src/python_pybind11/src/Vector3Stats.hh +++ b/src/python_pybind11/src/Vector3Stats.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ -#define IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ +#ifndef GZ_MATH_PYTHON__VECTOR3STATS_HH_ +#define GZ_MATH_PYTHON__VECTOR3STATS_HH_ #include @@ -30,7 +30,7 @@ namespace math { namespace python { -/// Define a pybind11 wrapper for an ignition::math::Vector3Stats +/// Define a pybind11 wrapper for an gz::math::Vector3Stats /** * \param[in] module a pybind11 module to add the definition to * \param[in] typestr name of the type used by Python @@ -40,4 +40,4 @@ void defineMathVector3Stats(py::module &m, const std::string &typestr); } // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ +#endif // GZ_MATH_PYTHON__VECTOR3STATS_HH_ diff --git a/src/python_pybind11/src/Vector4.cc b/src/python_pybind11/src/Vector4.cc index f50718756..a28d33c37 100644 --- a/src/python_pybind11/src/Vector4.cc +++ b/src/python_pybind11/src/Vector4.cc @@ -33,5 +33,5 @@ void defineMathVector4(py::module &m, const std::string &typestr) } } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition diff --git a/src/python_pybind11/src/Vector4.hh b/src/python_pybind11/src/Vector4.hh index 6db87b36b..b74d7e4c2 100644 --- a/src/python_pybind11/src/Vector4.hh +++ b/src/python_pybind11/src/Vector4.hh @@ -15,15 +15,15 @@ * */ -#ifndef IGNITION_MATH_PYTHON__VECTOR4_HH_ -#define IGNITION_MATH_PYTHON__VECTOR4_HH_ +#ifndef GZ_MATH_PYTHON__VECTOR4_HH_ +#define GZ_MATH_PYTHON__VECTOR4_HH_ #include #include #include -#include +#include namespace py = pybind11; using namespace pybind11::literals; @@ -34,14 +34,14 @@ namespace math { namespace python { -/// Help define a pybind11 wrapper for an ignition::math::Vector4 +/// Help define a pybind11 wrapper for an gz::math::Vector4 /** * \param[in] module a pybind11 module to add the definition to */ template void helpDefineMathVector4(py::module &m, const std::string &typestr) { - using Class = ignition::math::Vector4; + using Class = gz::math::Vector4; auto toString = [](const Class &si) { std::stringstream stream; stream << si; @@ -145,13 +145,13 @@ void helpDefineMathVector4(py::module &m, const std::string &typestr) .def("__repr__", toString); } -/// Define a pybind11 wrapper for an ignition::math::Vector4 +/// Define a pybind11 wrapper for an gz::math::Vector4 /** * \param[in] module a pybind11 module to add the definition to */ void defineMathVector4(py::module &m, const std::string &typestr); } // namespace python -} // namespace gazebo +} // namespace math } // namespace ignition -#endif // IGNITION_MATH_PYTHON__VECTOR4_HH_ +#endif // GZ_MATH_PYTHON__VECTOR4_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index a7e623101..c88ff50ca 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -67,121 +67,121 @@ PYBIND11_MODULE(math, m) { m.doc() = "Ignition Math Python Library."; - ignition::math::python::defineMathAngle(m, "Angle"); + gz::math::python::defineMathAngle(m, "Angle"); - ignition::math::python::defineMathAxisAlignedBox(m, "AxisAlignedBox"); + gz::math::python::defineMathAxisAlignedBox(m, "AxisAlignedBox"); - ignition::math::python::defineMathCapsule(m, "Capsule"); + gz::math::python::defineMathCapsule(m, "Capsule"); - ignition::math::python::defineMathColor(m, "Color"); + gz::math::python::defineMathColor(m, "Color"); - ignition::math::python::defineMathDiffDriveOdometry( + gz::math::python::defineMathDiffDriveOdometry( m, "DiffDriveOdometry"); - ignition::math::python::defineMathEllipsoid( + gz::math::python::defineMathEllipsoid( m, "Ellipsoid"); - ignition::math::python::defineMathGaussMarkovProcess( + gz::math::python::defineMathGaussMarkovProcess( m, "GaussMarkovProcess"); - ignition::math::python::defineMathHelpers(m); + gz::math::python::defineMathHelpers(m); - ignition::math::python::defineMathKmeans(m, "Kmeans"); + gz::math::python::defineMathKmeans(m, "Kmeans"); - ignition::math::python::defineMathMaterial(m, "Material"); + gz::math::python::defineMathMaterial(m, "Material"); - ignition::math::python::defineMathMovingWindowFilter(m, "MovingWindowFilter"); + gz::math::python::defineMathMovingWindowFilter(m, "MovingWindowFilter"); - ignition::math::python::defineMathPID(m, "PID"); + gz::math::python::defineMathPID(m, "PID"); - ignition::math::python::defineMathRand(m, "Rand"); + gz::math::python::defineMathRand(m, "Rand"); - ignition::math::python::defineMathRollingMean(m, "RollingMean"); + gz::math::python::defineMathRollingMean(m, "RollingMean"); - ignition::math::python::defineMathSignalStats(m, "SignalStats"); - ignition::math::python::defineMathSignalStatistic(m, "SignalStatistic"); - ignition::math::python::defineMathSignalVariance(m, "SignalVariance"); - ignition::math::python::defineMathSignalMaximum(m, "SignalMaximum"); - ignition::math::python::defineMathSignalMinimum(m, "SignalMinimum"); - ignition::math::python::defineMathSignalMaxAbsoluteValue( + gz::math::python::defineMathSignalStats(m, "SignalStats"); + gz::math::python::defineMathSignalStatistic(m, "SignalStatistic"); + gz::math::python::defineMathSignalVariance(m, "SignalVariance"); + gz::math::python::defineMathSignalMaximum(m, "SignalMaximum"); + gz::math::python::defineMathSignalMinimum(m, "SignalMinimum"); + gz::math::python::defineMathSignalMaxAbsoluteValue( m, "SignalMaxAbsoluteValue"); - ignition::math::python::defineMathSignalRootMeanSquare( + gz::math::python::defineMathSignalRootMeanSquare( m, "SignalRootMeanSquare"); - ignition::math::python::defineMathSignalMean(m, "SignalMean"); + gz::math::python::defineMathSignalMean(m, "SignalMean"); - ignition::math::python::defineMathRotationSpline(m, "RotationSpline"); + gz::math::python::defineMathRotationSpline(m, "RotationSpline"); - ignition::math::python::defineMathVector3Stats(m, "Vector3Stats"); + gz::math::python::defineMathVector3Stats(m, "Vector3Stats"); - ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); + gz::math::python::defineMathSemanticVersion(m, "SemanticVersion"); - ignition::math::python::defineMathSphericalCoordinates( + gz::math::python::defineMathSphericalCoordinates( m, "SphericalCoordinates"); - ignition::math::python::defineMathSpline(m, "Spline"); + gz::math::python::defineMathSpline(m, "Spline"); - ignition::math::python::defineMathStopwatch(m, "Stopwatch"); + gz::math::python::defineMathStopwatch(m, "Stopwatch"); - ignition::math::python::defineMathTemperature(m, "Temperature"); + gz::math::python::defineMathTemperature(m, "Temperature"); - ignition::math::python::defineMathVector2(m, "Vector2"); + gz::math::python::defineMathVector2(m, "Vector2"); - ignition::math::python::defineMathVector3(m, "Vector3"); + gz::math::python::defineMathVector3(m, "Vector3"); - ignition::math::python::defineMathPlane(m, "Planed"); + gz::math::python::defineMathPlane(m, "Planed"); - ignition::math::python::defineMathBox(m, "Boxd"); - ignition::math::python::defineMathBox(m, "Boxf"); + gz::math::python::defineMathBox(m, "Boxd"); + gz::math::python::defineMathBox(m, "Boxf"); - ignition::math::python::defineMathVector4(m, "Vector4"); + gz::math::python::defineMathVector4(m, "Vector4"); - ignition::math::python::defineMathInterval(m, "Interval"); + gz::math::python::defineMathInterval(m, "Interval"); - ignition::math::python::defineMathRegion3(m, "Region3"); + gz::math::python::defineMathRegion3(m, "Region3"); - ignition::math::python::defineMathPolynomial3(m, "Polynomial3"); + gz::math::python::defineMathPolynomial3(m, "Polynomial3"); - ignition::math::python::defineMathLine2(m, "Line2"); + gz::math::python::defineMathLine2(m, "Line2"); - ignition::math::python::defineMathLine3(m, "Line3"); + gz::math::python::defineMathLine3(m, "Line3"); - ignition::math::python::defineMathMatrix3(m, "Matrix3"); + gz::math::python::defineMathMatrix3(m, "Matrix3"); - ignition::math::python::defineMathMatrix4(m, "Matrix4"); + gz::math::python::defineMathMatrix4(m, "Matrix4"); - ignition::math::python::defineMathMatrix6(m, "Matrix6"); + gz::math::python::defineMathMatrix6(m, "Matrix6"); - ignition::math::python::defineMathTriangle(m, "Triangle"); + gz::math::python::defineMathTriangle(m, "Triangle"); - ignition::math::python::defineMathTriangle3(m, "Triangle3"); + gz::math::python::defineMathTriangle3(m, "Triangle3"); - ignition::math::python::defineMathQuaternion(m, "Quaternion"); + gz::math::python::defineMathQuaternion(m, "Quaternion"); - ignition::math::python::defineMathOrientedBox(m, "OrientedBoxd"); + gz::math::python::defineMathOrientedBox(m, "OrientedBoxd"); - ignition::math::python::defineMathPose3(m, "Pose3"); + gz::math::python::defineMathPose3(m, "Pose3"); - ignition::math::python::defineMathMassMatrix3(m, "MassMatrix3"); + gz::math::python::defineMathMassMatrix3(m, "MassMatrix3"); - ignition::math::python::defineMathSphere(m, "Sphered"); + gz::math::python::defineMathSphere(m, "Sphered"); - ignition::math::python::defineMathCylinder(m, "Cylinderd"); + gz::math::python::defineMathCylinder(m, "Cylinderd"); - ignition::math::python::defineMathInertial(m, "Inertiald"); + gz::math::python::defineMathInertial(m, "Inertiald"); - ignition::math::python::defineMathFrustum(m, "Frustum"); + gz::math::python::defineMathFrustum(m, "Frustum"); - ignition::math::python::defineMathFilter(m, "Filter"); + gz::math::python::defineMathFilter(m, "Filter"); - ignition::math::python::defineMathBiQuad(m, "BiQuad"); + gz::math::python::defineMathBiQuad(m, "BiQuad"); - ignition::math::python::defineMathBiQuadVector3( + gz::math::python::defineMathBiQuadVector3( m, "BiQuadVector3"); - ignition::math::python::defineMathOnePole(m, "OnePole"); + gz::math::python::defineMathOnePole(m, "OnePole"); - ignition::math::python::defineMathOnePoleQuaternion( + gz::math::python::defineMathOnePoleQuaternion( m, "OnePoleQuaternion"); - ignition::math::python::defineMathOnePoleVector3( + gz::math::python::defineMathOnePoleVector3( m, "OnePoleVector3"); } diff --git a/src/ruby/Angle.i b/src/ruby/Angle.i index 190d11c8b..47f72905e 100644 --- a/src/ruby/Angle.i +++ b/src/ruby/Angle.i @@ -17,7 +17,7 @@ %module angle %{ -#include +#include %} namespace ignition diff --git a/src/ruby/AxisAlignedBox.i b/src/ruby/AxisAlignedBox.i index a8d66ff9d..9c2c50874 100644 --- a/src/ruby/AxisAlignedBox.i +++ b/src/ruby/AxisAlignedBox.i @@ -19,11 +19,11 @@ %{ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include %} %include "typemaps.i" diff --git a/src/ruby/Box.i b/src/ruby/Box.i index dbdd32b5f..2d801601b 100644 --- a/src/ruby/Box.i +++ b/src/ruby/Box.i @@ -17,14 +17,14 @@ %module box %{ -#include -#include -#include -#include -#include -#include - -#include "ignition/math/detail/WellOrderedVector.hh" +#include +#include +#include +#include +#include +#include + +#include "gz/math/detail/WellOrderedVector.hh" #include #include diff --git a/src/ruby/Color.i b/src/ruby/Color.i index d916614e2..9c4dba572 100644 --- a/src/ruby/Color.i +++ b/src/ruby/Color.i @@ -17,10 +17,10 @@ %module Color %{ -#include -#include -#include -#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/Cylinder.i b/src/ruby/Cylinder.i index a5226ae27..3799d932f 100644 --- a/src/ruby/Cylinder.i +++ b/src/ruby/Cylinder.i @@ -17,11 +17,11 @@ %module cylinder %{ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/DiffDriveOdometry.i b/src/ruby/DiffDriveOdometry.i index 216c511c1..efb7d5901 100644 --- a/src/ruby/DiffDriveOdometry.i +++ b/src/ruby/DiffDriveOdometry.i @@ -19,10 +19,10 @@ %{ #include #include -#include -#include -#include -#include +#include +#include +#include +#include %} %include "typemaps.i" diff --git a/src/ruby/Filter.i b/src/ruby/Filter.i index be9e70562..dfcef607e 100644 --- a/src/ruby/Filter.i +++ b/src/ruby/Filter.i @@ -17,11 +17,11 @@ %module filter %{ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include %} %import Quaternion.i diff --git a/src/ruby/Frustum.i b/src/ruby/Frustum.i index ee756eed1..435950b1b 100644 --- a/src/ruby/Frustum.i +++ b/src/ruby/Frustum.i @@ -17,12 +17,12 @@ %module frustum %{ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/GaussMarkovProcess.i b/src/ruby/GaussMarkovProcess.i index d70bb0836..412840e3e 100644 --- a/src/ruby/GaussMarkovProcess.i +++ b/src/ruby/GaussMarkovProcess.i @@ -17,7 +17,7 @@ %module gaussMarkovProcess %{ -#include +#include %} namespace ignition diff --git a/src/ruby/Helpers.i b/src/ruby/Helpers.i index 408cb549c..97f8d88f0 100644 --- a/src/ruby/Helpers.i +++ b/src/ruby/Helpers.i @@ -17,7 +17,7 @@ %module helpers %{ -#include +#include %} %include "std_string.i" diff --git a/src/ruby/Inertial.i b/src/ruby/Inertial.i index c85cdcdee..ee36cb1b2 100644 --- a/src/ruby/Inertial.i +++ b/src/ruby/Inertial.i @@ -17,10 +17,10 @@ %module inertial %{ -#include -#include -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Pose3.hh" +#include +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Pose3.hh" %} namespace ignition diff --git a/src/ruby/Kmeans.i b/src/ruby/Kmeans.i index c1c26ac10..5c7fd2ca8 100644 --- a/src/ruby/Kmeans.i +++ b/src/ruby/Kmeans.i @@ -17,9 +17,9 @@ %module kmeans %{ -#include -#include -#include +#include +#include +#include %} %include "std_vector.i" diff --git a/src/ruby/Line2.i b/src/ruby/Line2.i index 658c98ab4..78a0e8978 100644 --- a/src/ruby/Line2.i +++ b/src/ruby/Line2.i @@ -17,9 +17,9 @@ %module line2 %{ -#include -#include -#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/Line3.i b/src/ruby/Line3.i index 476abfced..82f5c7abc 100644 --- a/src/ruby/Line3.i +++ b/src/ruby/Line3.i @@ -17,9 +17,9 @@ %module line3 %{ -#include -#include -#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/MassMatrix3.i b/src/ruby/MassMatrix3.i index 6fea9dd2c..1727e525d 100644 --- a/src/ruby/MassMatrix3.i +++ b/src/ruby/MassMatrix3.i @@ -17,14 +17,14 @@ %module massmatrix3 %{ -#include "ignition/math/MassMatrix3.hh" +#include "gz/math/MassMatrix3.hh" -#include "ignition/math/Helpers.hh" -#include "ignition/math/Material.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Vector2.hh" -#include "ignition/math/Vector3.hh" -#include "ignition/math/Matrix3.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Vector2.hh" +#include "gz/math/Vector3.hh" +#include "gz/math/Matrix3.hh" %} namespace ignition diff --git a/src/ruby/Material.i b/src/ruby/Material.i index 74669b44e..80f08fcdf 100644 --- a/src/ruby/Material.i +++ b/src/ruby/Material.i @@ -17,10 +17,10 @@ %module material %{ -#include -#include -#include -#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/MaterialType.i b/src/ruby/MaterialType.i index 45864fc13..f7a10ccac 100644 --- a/src/ruby/MaterialType.i +++ b/src/ruby/MaterialType.i @@ -17,9 +17,9 @@ %module materialtype %{ -#include -#include -#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/Matrix3.i b/src/ruby/Matrix3.i index 5aa049134..22f8028a4 100644 --- a/src/ruby/Matrix3.i +++ b/src/ruby/Matrix3.i @@ -17,11 +17,11 @@ %module matrix3 %{ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/Matrix4.i b/src/ruby/Matrix4.i index 35f93b2e6..3a6238ef6 100644 --- a/src/ruby/Matrix4.i +++ b/src/ruby/Matrix4.i @@ -17,12 +17,12 @@ %module matrix4 %{ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/MovingWindowFilter.i b/src/ruby/MovingWindowFilter.i index ea6fc003d..bd9f26428 100644 --- a/src/ruby/MovingWindowFilter.i +++ b/src/ruby/MovingWindowFilter.i @@ -16,8 +16,8 @@ */ %module movingwindowfilter %{ -#include "ignition/math/MovingWindowFilter.hh" -#include "ignition/math/Vector3.hh" +#include "gz/math/MovingWindowFilter.hh" +#include "gz/math/Vector3.hh" %} namespace ignition diff --git a/src/ruby/OrientedBox.i b/src/ruby/OrientedBox.i index ca32b3121..6c6ee9587 100644 --- a/src/ruby/OrientedBox.i +++ b/src/ruby/OrientedBox.i @@ -18,14 +18,14 @@ %module orientedbox %{ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/PID.i b/src/ruby/PID.i index 452e73045..7861cfcc2 100644 --- a/src/ruby/PID.i +++ b/src/ruby/PID.i @@ -16,7 +16,7 @@ */ %module pid %{ - #include + #include %} %include "typemaps.i" diff --git a/src/ruby/Plane.i b/src/ruby/Plane.i index 0c35aa481..892e3fe91 100644 --- a/src/ruby/Plane.i +++ b/src/ruby/Plane.i @@ -17,13 +17,13 @@ %module plane %{ -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include %} diff --git a/src/ruby/Pose3.i b/src/ruby/Pose3.i index 5b94579ba..202cc2e8c 100644 --- a/src/ruby/Pose3.i +++ b/src/ruby/Pose3.i @@ -17,10 +17,10 @@ %module quaternion %{ - #include - #include - #include - #include + #include + #include + #include + #include %} %include "std_string.i" diff --git a/src/ruby/Quaternion.i b/src/ruby/Quaternion.i index 0bc6d70bf..c97f45e38 100644 --- a/src/ruby/Quaternion.i +++ b/src/ruby/Quaternion.i @@ -17,10 +17,10 @@ %module quaternion %{ -#include -#include -#include -#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/Rand.i b/src/ruby/Rand.i index 0e7aa25cf..3ef19f5a7 100644 --- a/src/ruby/Rand.i +++ b/src/ruby/Rand.i @@ -17,7 +17,7 @@ %module rand %{ -#include +#include %} namespace ignition diff --git a/src/ruby/RollingMean.i b/src/ruby/RollingMean.i index be96792d4..0788de76c 100644 --- a/src/ruby/RollingMean.i +++ b/src/ruby/RollingMean.i @@ -17,7 +17,7 @@ %module rollingMean %{ -#include +#include %} namespace ignition diff --git a/src/ruby/RotationSpline.i b/src/ruby/RotationSpline.i index 7f62e815d..8af437de5 100644 --- a/src/ruby/RotationSpline.i +++ b/src/ruby/RotationSpline.i @@ -17,9 +17,9 @@ %module rotationspline %{ -#include -#include -#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/SemanticVersion.i b/src/ruby/SemanticVersion.i index cb105adfd..9d2e2becd 100644 --- a/src/ruby/SemanticVersion.i +++ b/src/ruby/SemanticVersion.i @@ -17,7 +17,7 @@ %module semanticversion %{ - #include + #include %} %include "std_string.i" diff --git a/src/ruby/SignalStats.i b/src/ruby/SignalStats.i index b918d3f35..406f922f8 100644 --- a/src/ruby/SignalStats.i +++ b/src/ruby/SignalStats.i @@ -18,7 +18,7 @@ %module signalStats %{ -#include +#include %} %include "std_string.i" diff --git a/src/ruby/Sphere.i b/src/ruby/Sphere.i index c2fa4b1a4..3c67ab3d5 100644 --- a/src/ruby/Sphere.i +++ b/src/ruby/Sphere.i @@ -17,11 +17,11 @@ %module sphere %{ -#include "ignition/math/Sphere.hh" -#include "ignition/math/MassMatrix3.hh" -#include "ignition/math/Material.hh" -#include "ignition/math/Quaternion.hh" -#include "ignition/math/Plane.hh" +#include "gz/math/Sphere.hh" +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" +#include "gz/math/Plane.hh" %} %include "typemaps.i" diff --git a/src/ruby/SphericalCoordinates.i b/src/ruby/SphericalCoordinates.i index 6b006f173..ce98ad306 100644 --- a/src/ruby/SphericalCoordinates.i +++ b/src/ruby/SphericalCoordinates.i @@ -20,10 +20,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/Spline.i b/src/ruby/Spline.i index cf086a577..f68c181ae 100644 --- a/src/ruby/Spline.i +++ b/src/ruby/Spline.i @@ -17,10 +17,10 @@ %module spline %{ -#include -#include -#include -#include +#include +#include +#include +#include %} namespace ignition diff --git a/src/ruby/StopWatch.i b/src/ruby/StopWatch.i index 161bb6105..7a640767d 100644 --- a/src/ruby/StopWatch.i +++ b/src/ruby/StopWatch.i @@ -19,9 +19,9 @@ %{ #include #include -#include "ignition/math/Stopwatch.hh" -#include -#include +#include "gz/math/Stopwatch.hh" +#include +#include %} %include "typemaps.i" diff --git a/src/ruby/Temperature.i b/src/ruby/Temperature.i index 27043ea62..8c538c647 100644 --- a/src/ruby/Temperature.i +++ b/src/ruby/Temperature.i @@ -17,7 +17,7 @@ %module temperature %{ -#include +#include %} %include "std_string.i" diff --git a/src/ruby/Triangle.i b/src/ruby/Triangle.i index 08c5f5370..966b29e7e 100644 --- a/src/ruby/Triangle.i +++ b/src/ruby/Triangle.i @@ -17,12 +17,12 @@ %module triangle %{ -#include -#include -#include +#include +#include +#include #include -#include -#include +#include +#include %} namespace ignition diff --git a/src/ruby/Triangle3.i b/src/ruby/Triangle3.i index 78c20e10b..2ad52da92 100644 --- a/src/ruby/Triangle3.i +++ b/src/ruby/Triangle3.i @@ -17,12 +17,12 @@ %module triangle3 %{ -#include -#include -#include +#include +#include +#include #include -#include -#include +#include +#include %} namespace ignition diff --git a/src/ruby/Vector2.i b/src/ruby/Vector2.i index cad4128ea..055bca283 100644 --- a/src/ruby/Vector2.i +++ b/src/ruby/Vector2.i @@ -23,7 +23,7 @@ %module vector2 %{ -#include +#include %} namespace ignition diff --git a/src/ruby/Vector3.i b/src/ruby/Vector3.i index 0acf12da5..09188b53a 100644 --- a/src/ruby/Vector3.i +++ b/src/ruby/Vector3.i @@ -23,7 +23,7 @@ %module vector3 %{ -#include +#include %} namespace ignition diff --git a/src/ruby/Vector3Stats.i b/src/ruby/Vector3Stats.i index 460733282..ba0060cb4 100644 --- a/src/ruby/Vector3Stats.i +++ b/src/ruby/Vector3Stats.i @@ -17,10 +17,10 @@ %module vector3stats %{ -#include -#include -#include -#include +#include +#include +#include +#include %} %include "std_string.i" diff --git a/src/ruby/Vector4.i b/src/ruby/Vector4.i index a9235cae3..7ae792253 100644 --- a/src/ruby/Vector4.i +++ b/src/ruby/Vector4.i @@ -23,7 +23,7 @@ %module vector4 %{ -#include +#include %} namespace ignition diff --git a/test/integration/deprecated_TEST.cc b/test/integration/deprecated_TEST.cc new file mode 100644 index 000000000..9def0ba9f --- /dev/null +++ b/test/integration/deprecated_TEST.cc @@ -0,0 +1,27 @@ +/* + * 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 + +///////////////////////////////////////////////// +// Make sure the ignition namespace still works +TEST(Deprecated, IgnitionNamespace) +{ + ignition::math::Angle angle; + (void) angle; +} diff --git a/tutorials/cppgetstarted.md b/tutorials/cppgetstarted.md index cffc8fe62..c9f1fed20 100644 --- a/tutorials/cppgetstarted.md +++ b/tutorials/cppgetstarted.md @@ -24,7 +24,7 @@ For this example, we'll take the short and easy approach. At this point your main file should look like ```{.cpp} -#include +#include int main() { @@ -38,7 +38,7 @@ ignition::math::Vector3d type which is a typedef of `Vector3`. The resul addition will be a main file similar to the following. ```{.cpp} -#include +#include int main() { @@ -53,7 +53,7 @@ Finally, we can compute the distance between `point1` and `point2` using the ignition::math::Vector3::Distance() function and output the distance value. ```{.cpp} -#include +#include int main() {