diff --git a/CMakeLists.txt b/CMakeLists.txt index bb963593b..6d7e08cef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,9 @@ find_package(ignition-cmake3 REQUIRED) #============================================================================ set (c++standard 17) set (CMAKE_CXX_STANDARD 17) -ign_configure_project(VERSION_SUFFIX pre1) +ign_configure_project( + REPLACE_IGNITION_INCLUDE_PATH gz/math + VERSION_SUFFIX pre1) #============================================================================ # Set project-specific options diff --git a/eigen3/BUILD.bazel b/eigen3/BUILD.bazel index e9ffd8f70..823745fc9 100644 --- a/eigen3/BUILD.bazel +++ b/eigen3/BUILD.bazel @@ -11,7 +11,7 @@ package( licenses(["notice"]) public_headers = glob([ - "include/ignition/math/eigen3/*.hh", + "include/gz/math/eigen3/*.hh", ]) cc_library( 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..ac8e27cf9 --- /dev/null +++ b/eigen3/include/gz/math/eigen3/Conversions.hh @@ -0,0 +1,167 @@ +/* + * 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 + +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 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::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 diff --git a/eigen3/include/gz/math/eigen3/Util.hh b/eigen3/include/gz/math/eigen3/Util.hh new file mode 100644 index 000000000..bfc49c7f8 --- /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 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 diff --git a/eigen3/include/ignition/math/eigen3.hh b/eigen3/include/ignition/math/eigen3.hh new file mode 100644 index 000000000..518f8fb7f --- /dev/null +++ b/eigen3/include/ignition/math/eigen3.hh @@ -0,0 +1,18 @@ +/* + * 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. + * + */ + +#include diff --git a/eigen3/include/ignition/math/eigen3/Conversions.hh b/eigen3/include/ignition/math/eigen3/Conversions.hh index 2dcd1b09f..3931146f5 100644 --- a/eigen3/include/ignition/math/eigen3/Conversions.hh +++ b/eigen3/include/ignition/math/eigen3/Conversions.hh @@ -13,155 +13,6 @@ * 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 - -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 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::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 diff --git a/eigen3/include/ignition/math/eigen3/Util.hh b/eigen3/include/ignition/math/eigen3/Util.hh index 25f740bd8..8e1172782 100644 --- a/eigen3/include/ignition/math/eigen3/Util.hh +++ b/eigen3/include/ignition/math/eigen3/Util.hh @@ -13,149 +13,6 @@ * 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 diff --git a/examples/additively_separable_scalar_field3_example.cc b/examples/additively_separable_scalar_field3_example.cc index c46ffd5ee..6197d5480 100644 --- a/examples/additively_separable_scalar_field3_example.cc +++ b/examples/additively_separable_scalar_field3_example.cc @@ -16,8 +16,8 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { diff --git a/examples/angle_example.cc b/examples/angle_example.cc index eb2ea916c..635499714 100644 --- a/examples/angle_example.cc +++ b/examples/angle_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/color_example.cc b/examples/color_example.cc index 4d7325a9e..d7c340acf 100644 --- a/examples/color_example.cc +++ b/examples/color_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/diff_drive_odometry.cc b/examples/diff_drive_odometry.cc index 6f2bcdf8e..f244ace8d 100644 --- a/examples/diff_drive_odometry.cc +++ b/examples/diff_drive_odometry.cc @@ -18,9 +18,9 @@ #include #include -#include -#include -#include +#include +#include +#include int main(int argc, char **argv) { diff --git a/examples/gauss_markov_process_example.cc b/examples/gauss_markov_process_example.cc index 313bd6272..027ea5136 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. diff --git a/examples/graph_example.cc b/examples/graph_example.cc index a30788d48..968a4513a 100644 --- a/examples/graph_example.cc +++ b/examples/graph_example.cc @@ -16,7 +16,7 @@ */ #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/helpers_example.cc b/examples/helpers_example.cc index bf22643f9..f3858b7a6 100644 --- a/examples/helpers_example.cc +++ b/examples/helpers_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/interval_example.cc b/examples/interval_example.cc index 68623ca72..9b1a4fea2 100644 --- a/examples/interval_example.cc +++ b/examples/interval_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/kmeans.cc b/examples/kmeans.cc index 6f02d2091..209948551 100644 --- a/examples/kmeans.cc +++ b/examples/kmeans.cc @@ -18,7 +18,7 @@ #include #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/matrix3_example.cc b/examples/matrix3_example.cc index a3f7d3168..60fd32b69 100644 --- a/examples/matrix3_example.cc +++ b/examples/matrix3_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/piecewise_scalar_field3_example.cc b/examples/piecewise_scalar_field3_example.cc index d584ff09c..1da7144f8 100644 --- a/examples/piecewise_scalar_field3_example.cc +++ b/examples/piecewise_scalar_field3_example.cc @@ -17,9 +17,9 @@ //! [complete] #include -#include -#include -#include +#include +#include +#include int main(int argc, char **argv) { diff --git a/examples/polynomial3_example.cc b/examples/polynomial3_example.cc index f268dbdec..8323c24bd 100644 --- a/examples/polynomial3_example.cc +++ b/examples/polynomial3_example.cc @@ -16,8 +16,8 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { diff --git a/examples/pose3_example.cc b/examples/pose3_example.cc index 365436032..9a3d830f0 100644 --- a/examples/pose3_example.cc +++ b/examples/pose3_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/quaternion_example.cc b/examples/quaternion_example.cc index e9cbbccaf..682e75575 100644 --- a/examples/quaternion_example.cc +++ b/examples/quaternion_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/quaternion_from_euler.cc b/examples/quaternion_from_euler.cc index 0f3fb16c8..4c1d5cb52 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) diff --git a/examples/quaternion_to_euler.cc b/examples/quaternion_to_euler.cc index 9b3ea41f6..eb2f18c12 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) diff --git a/examples/rand_example.cc b/examples/rand_example.cc index 227e4399a..6374c7d42 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. diff --git a/examples/region3_example.cc b/examples/region3_example.cc index 1cb647c01..92a6a78a5 100644 --- a/examples/region3_example.cc +++ b/examples/region3_example.cc @@ -16,8 +16,8 @@ */ //! [complete] #include -#include -#include +#include +#include int main(int argc, char **argv) { diff --git a/examples/temperature_example.cc b/examples/temperature_example.cc index f81498a52..9c6892b80 100644 --- a/examples/temperature_example.cc +++ b/examples/temperature_example.cc @@ -16,7 +16,7 @@ */ //! [complete] #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/triangle_example.cc b/examples/triangle_example.cc index a2adeaf99..e8e4acd06 100644 --- a/examples/triangle_example.cc +++ b/examples/triangle_example.cc @@ -16,7 +16,7 @@ */ #include -#include +#include int main(int argc, char **argv) { diff --git a/examples/vector2_example.cc b/examples/vector2_example.cc index e558a2327..3238135b0 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) { 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..6ddf17cfb --- /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\ + * gz/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/gz/math/Angle.hh b/include/gz/math/Angle.hh new file mode 100644 index 000000000..3348f7dd3 --- /dev/null +++ b/include/gz/math/Angle.hh @@ -0,0 +1,247 @@ +/* + * 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 +#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 gz/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 Π + + /// \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() = default; + + /// \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: constexpr Angle(double _radian) + : value(_radian) + { + } + + /// \brief Set the value from an angle in radians. + /// \param[in] _radian Radian value. + /// \deprecated Use void SetRadian(double) + public: void IGN_DEPRECATED(7) 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 + /// \deprecated Use void SetDegree(double) + public: void IGN_DEPRECATED(7) 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, with a default value of zero. + 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..a59bc5f04 --- /dev/null +++ b/include/gz/math/AxisAlignedBox.hh @@ -0,0 +1,249 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \class AxisAlignedBox AxisAlignedBox.hh gz/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 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 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 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 + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Box.hh b/include/gz/math/Box.hh new file mode 100644 index 000000000..c0f8e69fb --- /dev/null +++ b/include/gz/math/Box.hh @@ -0,0 +1,222 @@ +/* + * 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 gz/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 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 "gz/math/detail/Box.hh" +#endif diff --git a/include/ignition/math/CMakeLists.txt b/include/gz/math/CMakeLists.txt similarity index 90% rename from include/ignition/math/CMakeLists.txt rename to include/gz/math/CMakeLists.txt index 4199793d8..ef62a88a5 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/${IGN_DESIGNATION}) +install(DIRECTORY detail DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}/gz/${IGN_DESIGNATION}) diff --git a/include/gz/math/Capsule.hh b/include/gz/math/Capsule.hh new file mode 100644 index 000000000..513f8f743 --- /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 gz/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..fd4355d83 --- /dev/null +++ b/include/gz/math/Color.hh @@ -0,0 +1,392 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Color Color.hh gz/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() = default; + + /// \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: constexpr Color(const float _r, const float _g, const float _b, + const float _a = 1.0) + : r(_r), g(_g), b(_b), a(_a) + { + this->Clamp(); + } + + /// \brief Copy Constructor + /// \param[in] _clr Color to copy + public: Color(const Color &_clr) = default; + + /// \brief Destructor + public: ~Color() = default; + + /// \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) = default; + + /// \brief Array index operator + /// \param[in] _index Color component index(0=red, 1=green, 2=blue, + /// 3=alpha) + /// \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 Array index operator, const version + /// \param[in] _index Color component index(0=red, 1=green, 2=blue, + /// 3=alpha) + /// \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) const; + + /// \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: constexpr void Clamp() + { + // These comparisons are carefully written to handle NaNs correctly. + if (!(this->r >= 0)) { this->r = 0; } + if (!(this->g >= 0)) { this->g = 0; } + if (!(this->b >= 0)) { this->b = 0; } + if (!(this->a >= 0)) { this->a = 0; } + if (this->r > 1) { this->r = this->r/255.0f; } + if (this->g > 1) { this->g = this->g/255.0f; } + if (this->b > 1) { this->b = this->b/255.0f; } + if (this->a > 1) { this->a = 1; } + } + + /// \brief Stream insertion operator + /// \param[in] _out the output stream + /// \param[in] _color the color + /// \return the output stream + public: friend std::ostream &operator<<(std::ostream &_out, + const Color &_color) + { + for (auto i : {0, 1, 2, 3}) + { + if (i > 0) + _out << " "; + + appendToStream(_out, _color[i]); + } + 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..69e6d88d3 --- /dev/null +++ b/include/gz/math/Cylinder.hh @@ -0,0 +1,180 @@ +/* + * 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 gz/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 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..398358a85 --- /dev/null +++ b/include/gz/math/DiffDriveOdometry.hh @@ -0,0 +1,140 @@ +/* + * 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 { + /** \class DiffDriveOdometry DiffDriveOdometry.hh \ + * gz/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 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); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/gz/math/Ellipsoid.hh b/include/gz/math/Ellipsoid.hh new file mode 100644 index 000000000..a2e2a1e52 --- /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 gz/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..5e35befd2 --- /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 gz/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 gz/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 gz/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 gz/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 gz/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 gz/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..4a4884fee --- /dev/null +++ b/include/gz/math/Frustum.hh @@ -0,0 +1,172 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + + /// \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(double _near, + double _far, + const math::Angle &_fov, + double _aspectRatio, + const math::Pose3d &_pose = math::Pose3d::Zero); + + /// \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(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(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(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 Compute the planes of the frustum. This is called whenever + /// a property of the frustum is changed. + private: void ComputePlanes(); + + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/GaussMarkovProcess.hh b/include/gz/math/GaussMarkovProcess.hh new file mode 100644 index 000000000..83173ef71 --- /dev/null +++ b/include/gz/math/GaussMarkovProcess.hh @@ -0,0 +1,137 @@ +/* + * 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 { + /** \class GaussMarkovProcess GaussMarkovProcess.hh\ + * gz/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 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); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh new file mode 100644 index 000000000..85de8b08d --- /dev/null +++ b/include/gz/math/Helpers.hh @@ -0,0 +1,868 @@ +/* + * 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 "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 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(); + + /// \brief Simple clamping function that constrains a value to + /// a range defined by a min and max value. This function is equivalent to + /// std::max(std::min(value, max), min). + /// \param[in] _v Value to clamp + /// \param[in] _min Minimum allowed value. + /// \param[in] _max Maximum allowed value. + /// \return The value _v clamped to the range defined by _min and _max. + 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 to check. + /// \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 to check + /// \return True if _v is not a number, false otherwise. + inline bool isnan(double _v) + { + return (std::isnan)(_v); + } + + /// \brief Fix a float NaN value. + /// \param[in] _v Value to correct. + /// \return 0 if _v is NaN or infinite, _v otherwise. + inline float fixnan(float _v) + { + return isnan(_v) || std::isinf(_v) ? 0.0f : _v; + } + + /// \brief Fix a double NaN value. + /// \param[in] _v Value to correct. + /// \return 0 if _v is NaN or is infinite, _v otherwise. + inline double fixnan(double _v) + { + return isnan(_v) || std::isinf(_v) ? 0.0 : _v; + } + + /// \brief Check if an int 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 an unsigned int 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 an int 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 an unsigned int 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 value in a vector of values + /// \param[in] _values The vector of values. + /// \return The mean value in the provided vector. + 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 the variance of a vector of values. + /// \param[in] _values The vector of values. + /// \return The squared deviation of the vector of values. + 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 value in the vector. + 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 value in the vector. + 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 + /// \return True if the two values fall within the given 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 Less than or near test, within a tolerance. + /// \param[in] _a The first value. + /// \param[in] _b The second value. + /// \param[in] _epsilon The tolerance. + /// \return True if _a < _b + _tol. + template + inline bool lessOrNearEqual(const T &_a, const T &_b, + const T &_epsilon = 1e-6) + { + return _a < _b + _epsilon; + } + + /// \brief Greater than or near test, within a tolerance. + /// \param[in] _a The first value. + /// \param[in] _b The second value. + /// \param[in] _epsilon The tolerance. + /// \return True if _a > _b - _epsilon. + template + inline bool greaterOrNearEqual(const T &_a, const T &_b, + const T &_epsilon = 1e-6) + { + return _a > _b - _epsilon; + } + + /// \brief Get the 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[in, out] _a The first number. This variable will contain the + /// lower of the two values after this function completes. + /// \param[in, out] _b The second number. This variable will contain the + /// higher of the two values after this function completes. + 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[in,out] _a The first number. This variable will contain the + /// lowest of the three values after this function completes. + /// \param[in,out] _b The second number. This variable will contain the + /// middle of the three values after this function completes. + /// \param[in,out] _c The third number. This variable will contain the + /// highest of the three values after this function completes. + 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. + /// \param[in] _precision Precision for floating point numbers. + /// \deprecated Use appendToStream(std::ostream, T) instead. + template + inline void IGN_DEPRECATED(7) appendToStream( + std::ostream &_out, T _number, int _precision) + { + if (std::fpclassify(_number) == FP_ZERO) + { + _out << 0; + } + else + { + _out << precision(_number, _precision); + } + } + + /// \brief Append a number to a stream, specialized for int. + /// \param[out] _out Output stream. + /// \param[in] _number Number to append. + /// _precision Not used for int. + /// \deprecated Use appendToStream(std::ostream, int) instead. + template<> + inline void IGN_DEPRECATED(7) appendToStream( + std::ostream &_out, int _number, int) + { + _out << _number; + } + + /// \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 the parameter a power of 2? + /// \param[in] _x The number to check. + /// \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 than or equal to + /// a given value. + /// \param[in] _x The value which marks the lower bound of the result. + /// \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 input string. + /// \return An integer, or NAN_I if unable to parse the input. + 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 [in] _input The string. + /// \return A floating point number (can be NaN) or NAN_D if the + /// _input could not be parsed. + 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 + std::string IGNITION_MATH_VISIBLE timePointToString( + const std::chrono::steady_clock::time_point &_point); + + /// \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 + std::string IGNITION_MATH_VISIBLE durationToString( + const std::chrono::steady_clock::duration &_duration); + + /// \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; + + // 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 + + // `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 + bool IGNITION_MATH_VISIBLE splitTimeBasedOnTimeRegex( + const std::string &_timeString, + uint64_t & numberDays, uint64_t & numberHours, + uint64_t & numberMinutes, uint64_t & numberSeconds, + uint64_t & numberMilliseconds); + + /// \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. + std::chrono::steady_clock::duration IGNITION_MATH_VISIBLE stringToDuration( + const std::string &_timeString); + + /// \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. + std::chrono::steady_clock::time_point + IGNITION_MATH_VISIBLE stringToTimePoint(const std::string &_timeString); + + // 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 implementation 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..31a01f6dc --- /dev/null +++ b/include/gz/math/Inertial.hh @@ -0,0 +1,269 @@ +/* + * 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 gz/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) = default; + + /// \brief Destructor. + public: ~Inertial() = default; + + /// \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; + } + + /// \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) = default; + + /// \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..c804887f1 --- /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 gz/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/gz/math/Kmeans.hh b/include/gz/math/Kmeans.hh new file mode 100644 index 000000000..438a2af5a --- /dev/null +++ b/include/gz/math/Kmeans.hh @@ -0,0 +1,85 @@ +/* + * 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 + +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + + /// \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 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; + + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/gz/math/Line2.hh b/include/gz/math/Line2.hh new file mode 100644 index 000000000..a7203916f --- /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 gz/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..911c29e59 --- /dev/null +++ b/include/gz/math/Line3.hh @@ -0,0 +1,414 @@ +/* + * 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 gz/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) = default; + + /// \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) = default; + + /// \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..6559dd20c --- /dev/null +++ b/include/gz/math/MassMatrix3.hh @@ -0,0 +1,1104 @@ +/* + * 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 gz/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) = default; + + /// \brief Destructor. + public: ~MassMatrix3() = default; + + /// \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. + 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. + 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. + public: bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) + { + this->Ixyxzyz = _ixyxzyz; + return this->IsValid(); + } + + /// \brief Get IXX + /// \return IXX value + public: T Ixx() const + { + return this->Ixxyyzz[0]; + } + + /// \brief Get IYY + /// \return IYY value + public: T Iyy() const + { + return this->Ixxyyzz[1]; + } + + /// \brief Get IZZ + /// \return IZZ value + public: T Izz() const + { + return this->Ixxyyzz[2]; + } + + /// \brief Get IXY + /// \return IXY value + public: T Ixy() const + { + return this->Ixyxzyz[0]; + } + + /// \brief Get IXZ + /// \return IXZ value + public: T Ixz() const + { + return this->Ixyxzyz[1]; + } + + /// \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. + 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. + 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. + 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. + 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. + 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. + 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 + 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. + 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) + = default; + + /// \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 + { + // Compute tolerance relative to maximum value of inertia diagonal + T tol = _tol * this->Ixxyyzz.Max(); + Vector3 moments = this->PrincipalMoments(tol); + 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..7fd6a82a2 --- /dev/null +++ b/include/gz/math/Material.hh @@ -0,0 +1,142 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + + /// \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/gz/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 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 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. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/MaterialType.hh b/include/gz/math/MaterialType.hh new file mode 100644 index 000000000..5a1355252 --- /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_MATERIALTYPE_HH_ +#define GZ_MATH_MATERIALTYPE_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..680d88f7b --- /dev/null +++ b/include/gz/math/Matrix3.hh @@ -0,0 +1,675 @@ +/* + * 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 gz/math/Matrix3.hh + /// \brief A 3x3 matrix class. + /// + /// The following two type definitions are provided: + /// + /// * \ref Matrix3i : Equivalent to Matrix3 + /// * \ref Matrix3f : Equivalent to Matrix3 + /// * \ref Matrix3d : Equivalent to Matrix3 + /// ## Examples + /// + /// * C++ + /// + /// \snippet examples/matrix3_example.cc complete + /// + /// * Ruby + /// \code{.rb} + /// # Modify the RUBYLIB environment variable to include the ignition math + /// # library install path. For example, if you install to /user: + /// # + /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB + /// # + /// require 'gz/math' + /// + /// # Construct a default matrix3. + /// m = Ignition::Math::Matrix3d.new + /// printf("The default constructed matrix m has the following "+ + /// "values.\n\t" + + /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + /// m.(0, 0), m.(0, 1), m.(0, 2), + /// m.(1, 0), m.(1, 1), m.(1, 2), + /// m.(2, 0), m.(2, 1), m.(2, 2)) + /// + /// # Set the first column of the matrix. + /// m.SetCol(0, Ignition::Math::Vector3d.new(3, 4, 5)) + /// printf("Setting the first column of the matrix m to 3, 4, 5.\n\t" + + /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + /// m.(0, 0), m.(0, 1), m.(0, 2), + /// m.(1, 0), m.(1, 1), m.(1, 2), + /// m.(2, 0), m.(2, 1), m.(2, 2)) + /// + /// # Transpose the matrix. + /// t = m.Transposed() + /// printf("The transposed matrix t has the values.\n\t"+ + /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + /// t.(0, 0), t.(0, 1), t.(0, 2), + /// t.(1, 0), t.(1, 1), t.(1, 2), + /// t.(2, 0), t.(2, 1), t.(2, 2)) + /// + /// # Multiply the two matrices. + /// m = m * t + /// printf("m * t = " + + /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", + /// m.(0, 0), m.(0, 1), m.(0, 2), + /// m.(1, 0), m.(1, 1), m.(1, 2), + /// m.(2, 0), m.(2, 1), m.(2, 2)) + /// \endcode + template + class Matrix3 + { + /// \brief A Matrix3 initialized to identity. + /// This is equivalent to math::Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1). + public: static const Matrix3 &Identity; + + /// \brief A Matrix3 initialized to zero. + /// This is equivalent to math::Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0). + public: static const Matrix3 &Zero; + + /// \brief Default constructor that initializes the matrix3 to zero. + public: Matrix3() + { + std::memset(this->data, 0, sizeof(this->data[0][0])*9); + } + + /// \brief Copy constructor. + /// \param _m Matrix to copy + public: Matrix3(const Matrix3 &_m) = default; + + /// \brief Construct a matrix3 using nine values. + /// \param[in] _v00 Row 0, Col 0 value + /// \param[in] _v01 Row 0, Col 1 value + /// \param[in] _v02 Row 0, Col 2 value + /// \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: constexpr Matrix3(T _v00, T _v01, T _v02, + T _v10, T _v11, T _v12, + T _v20, T _v21, T _v22) + : data{{_v00, _v01, _v02}, + {_v10, _v11, _v12}, + {_v20, _v21, _v22}} + { + } + + /// \brief Construct 3x3 rotation Matrix from a quaternion. + /// \param[in] _q Quaternion to set the Matrix3 from. + 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: ~Matrix3() = default; + + /// \brief Set a single value. + /// \param[in] _row row index. _row is clamped to the range [0,2] + /// \param[in] _col column index. _col is clamped to the range [0,2] + /// \param[in] _v New value. + public: void Set(size_t _row, size_t _col, T _v) + { + this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] + [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] = _v; + } + + /// \brief Set values. + /// \param[in] _v00 Row 0, Col 0 value + /// \param[in] _v01 Row 0, Col 1 value + /// \param[in] _v02 Row 0, Col 2 value + /// \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, the first column of the matrix. + /// \param[in] _yAxis The y axis, the second column of the matrix. + /// \param[in] _zAxis The z axis, the third column of the matrix. + /// \deprecated Use SetAxes(const Vector3 &, const Vector3 &, + /// const Vector3 &,) + public: void IGN_DEPRECATED(7) Axes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis) + { + this->SetAxes(_xAxis, _yAxis, _zAxis); + } + + /// \brief Set the matrix from three axis (1 per column). + /// \param[in] _xAxis The x axis, the first column of the matrix. + /// \param[in] _yAxis The y axis, the second column of the matrix. + /// \param[in] _zAxis The z axis, the third column of the matrix. + public: void SetAxes(const Vector3 &_xAxis, + const Vector3 &_yAxis, + const Vector3 &_zAxis) + { + this->SetCol(0, _xAxis); + this->SetCol(1, _yAxis); + this->SetCol(2, _zAxis); + } + + /// \brief Set as a rotation matrix from an axis and angle. + /// \param[in] _axis the axis + /// \param[in] _angle ccw rotation around the axis in radians + /// \deprecated Use SetFromAxisAngle(const Vector3 &, T) + public: void IGN_DEPRECATED(7) Axis(const Vector3 &_axis, T _angle) + { + this->SetFromAxisAngle(_axis, _angle); + } + + /// \brief Set as a rotation matrix from an axis and angle. + /// \param[in] _axis the axis + /// \param[in] _angle ccw rotation around the axis in radians + public: void SetFromAxisAngle(const Vector3 &_axis, T _angle) + { + T c = cos(_angle); + T s = sin(_angle); + 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 as a rotation 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 + /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) + public: void IGN_DEPRECATED(7) From2Axes( + const Vector3 &_v1, const Vector3 &_v2) + { + this->SetFrom2Axes(_v1, _v2); + } + + /// \brief Set as a rotation matrix to represent rotation from + /// vector _v1 to vector _v2, so that + /// _v2.Normalize() == this * _v1.Normalize() holds. + /// + /// \param[in] _v1 The first vector + /// \param[in] _v2 The second vector + public: void SetFrom2Axes(const Vector3 &_v1, const Vector3 &_v2) + { + const T _v1LengthSquared = _v1.SquaredLength(); + if (_v1LengthSquared <= 0.0) + { + // 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->SetFromAxisAngle(cross, acos(dot)); + } + + /// \brief Set a column. + /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the + /// range [0, 2]. + /// \param[in] _v The value to set in each row of the column. + /// \deprecated Use SetCol(unsigned int _c, const Vector3 &_v) + public: void IGN_DEPRECATED(7) Col(unsigned int _c, const Vector3 &_v) + { + this->SetCol(_c, _v); + } + + /// \brief Set a column. + /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the + /// range [0, 2]. + /// \param[in] _v The value to set in each row of the column. + public: void SetCol(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 Matrix to copy. + /// \return This matrix. + public: Matrix3 &operator=(const Matrix3 &_mat) = default; + + /// \brief Subtraction operator. + /// \param[in] _m Matrix to subtract. + /// \return The element wise difference of two matrices. + public: Matrix3 operator-(const Matrix3 &_m) const + { + return Matrix3( + 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 Addition operation. + /// \param[in] _m Matrix to add. + /// \return The element wise sum of two matrices + public: Matrix3 operator+(const Matrix3 &_m) const + { + return Matrix3( + 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 Scalar multiplication operator. + /// \param[in] _s Value to multiply. + /// \return 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 as a 3x3 rotation matrix 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 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. This operator outputs all + /// 9 scalar values in the matrix separated by a single space. + /// \param[in, out] _out Output stream. + /// \param[in] _m Matrix to output. + /// \return The stream. + public: friend std::ostream &operator<<( + std::ostream &_out, const ignition::math::Matrix3 &_m) + { + for (auto i : {0, 1, 2}) + { + for (auto j : {0, 1, 2}) + { + if (!(i == 0 && j == 0)) + _out << " "; + + appendToStream(_out, _m(i, j)); + } + } + + return _out; + } + + /// \brief Stream extraction operator. This operator requires 9 space + /// separated scalar values, such as "1 2 3 4 5 6 7 8 9". + /// \param [in, out] _in Input stream. + /// \param [out] _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]; + }; + + namespace detail { + + template + constexpr Matrix3 gMatrix3Identity( + 1, 0, 0, + 0, 1, 0, + 0, 0, 1); + + template + constexpr Matrix3 gMatrix3Zero( + 0, 0, 0, + 0, 0, 0, + 0, 0, 0); + + } // namespace detail + + template + const Matrix3 &Matrix3::Identity = detail::gMatrix3Identity; + + template + const Matrix3 &Matrix3::Zero = detail::gMatrix3Zero; + + /// typedef Matrix3 as Matrix3i. + typedef Matrix3 Matrix3i; + + /// typedef Matrix3 as Matrix3d. + typedef Matrix3 Matrix3d; + + /// typedef Matrix3 as Matrix3f. + typedef Matrix3 Matrix3f; + } + } +} +#endif diff --git a/include/gz/math/Matrix4.hh b/include/gz/math/Matrix4.hh new file mode 100644 index 000000000..5a6b6b2f1 --- /dev/null +++ b/include/gz/math/Matrix4.hh @@ -0,0 +1,880 @@ +/* + * 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 gz/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) = 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] _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: constexpr 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) + : data{{_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: ~Matrix4() = default; + + /// \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 + 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. + 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.SetW(root / 2.0); + root = 1.0 / (2.0 * root); + q.SetX((this->data[2][1] - this->data[1][2]) * root); + q.SetY((this->data[0][2] - this->data[2][0]) * root); + q.SetZ((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.SetX(a); break; + case 1: q.SetY(a); break; + case 2: q.SetZ(a); break; + }; + switch (j) + { + default: + case 0: q.SetX(b); break; + case 1: q.SetY(b); break; + case 2: q.SetZ(b); break; + }; + switch (k) + { + default: + case 0: q.SetX(c); break; + case 1: q.SetY(c); break; + case 2: q.SetZ(c); break; + }; + + q.SetW((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 + /// \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) = default; + + /// \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) + { + for (auto i : {0, 1, 2, 3}) + { + for (auto j : {0, 1, 2, 3}) + { + 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 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]; + }; + + namespace detail { + + template + constexpr Matrix4 gMatrix4Identity( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); + + template + constexpr Matrix4 gMatrix4Zero( + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0); + + } // namespace detail + + template + const Matrix4 &Matrix4::Identity = detail::gMatrix4Identity; + + template + const Matrix4 &Matrix4::Zero = detail::gMatrix4Zero; + + typedef Matrix4 Matrix4i; + typedef Matrix4 Matrix4d; + typedef Matrix4 Matrix4f; + } + } +} +#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..63795c4ff --- /dev/null +++ b/include/gz/math/OrientedBox.hh @@ -0,0 +1,274 @@ +/* + * 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) = default; + + /// \brief Destructor + public: ~OrientedBox() = default; + + /// \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(const Vector3 &_size) + { + // Enforce non-negative size + this->size = _size.Abs(); + } + + /// \brief Set the box pose. + /// \param[in] _pose Box pose. + public: void Pose(const 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) = default; + + /// \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 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..e913d2beb --- /dev/null +++ b/include/gz/math/PID.hh @@ -0,0 +1,191 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class PID PID.hh gz/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 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 Reset the errors and command. + public: void Reset(); + + /// \brief Pointer to private data. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/PiecewiseScalarField3.hh b/include/gz/math/PiecewiseScalarField3.hh new file mode 100644 index 000000000..615a1e733 --- /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\ + * gz/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/gz/math/Plane.hh b/include/gz/math/Plane.hh new file mode 100644 index 000000000..e5d547527 --- /dev/null +++ b/include/gz/math/Plane.hh @@ -0,0 +1,282 @@ +/* + * 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 gz/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: explicit 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) = default; + + /// \brief Destructor + public: ~Plane() = default; + + /// \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) = default; + + /// \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..2eea5ae32 --- /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 gz/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/gz/math/Pose3.hh b/include/gz/math/Pose3.hh new file mode 100644 index 000000000..8eea645cc --- /dev/null +++ b/include/gz/math/Pose3.hh @@ -0,0 +1,560 @@ +/* + * 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 gz/math/Pose3.hh + /// \brief The Pose3 class represents a 3D position and rotation. The + /// position component is a Vector3, and the rotation is a Quaternion. + /// + /// The following two type definitions are provided: + /// + /// * \ref Pose3f + /// * \ref Pose3d + /// ## Examples + /// + /// * C++ + /// + /// \snippet examples/pose3_example.cc complete + /// + /// * Ruby + /// + /// \code{.rb} + /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB + /// # + /// require 'gz/math' + /// + /// # Construct a default Pose3d. + /// p = Ignition::Math::Pose3d.new + /// printf("A default Pose3d has the following values\n" + + /// "%f %f %f %f %f %f\n", p.Pos().X(), p.Pos().Y(), p.Pos().Z(), + /// p.Rot().Euler().X(), p.Rot().Euler().Y(), p.Rot().Euler().Z()) + /// + /// # Construct a pose at position 1, 2, 3 with a yaw of PI radians. + /// p1 = Ignition::Math::Pose3d.new(1, 2, 3, 0, 0, Math::PI) + /// printf("A pose3d(1, 2, 3, 0, 0, IGN_PI) has the following values\n" + + /// "%f %f %f %f %f %f\n", p1.Pos().X(), p1.Pos().Y(), p1.Pos().Z(), + /// p1.Rot().Euler().X(), p1.Rot().Euler().Y(), p1.Rot().Euler().Z()) + /// + /// # Set the position of a pose to 10, 20, 30 + /// p.Pos().Set(10, 20, 30) + /// + /// p3 = p * p1 + /// printf("Result of combining two poses is\n"+ + /// "%f %f %f %f %f %f\n", p3.Pos().X(), p3.Pos().Y(), p3.Pos().Z(), + /// p3.Rot().Euler().X(), p3.Rot().Euler().Y(), p3.Rot().Euler().Z()) + /// \endcode + template + class Pose3 + { + /// \brief A Pose3 initialized to zero. + /// This is equivalent to math::Pose3(0, 0, 0, 0, 0, 0). + public: static const Pose3 &Zero; + + /// \brief Default constructor. This initializes the position + /// component to zero and the quaternion to identity. + public: Pose3() = default; + + /// \brief Create a Pose3 based on a position and rotation. + /// \param[in] _pos The position + /// \param[in] _rot The rotation + public: Pose3(const Vector3 &_pos, const Quaternion &_rot) + : p(_pos), q(_rot) + { + } + + /// \brief Create a Pose3 using a 6-tuple consisting of + /// x, y, z, roll, pitch, and yaw. + /// \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 Create a Pose3 using a 7-tuple consisting of + /// x, y, z, qw, qx, qy, qz. The first three values are the position + /// and the last four the rotation represented as a quaternion. + /// \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) = default; + + /// \brief Destructor. + public: ~Pose3() = default; + + /// \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 a position and Euler angles. + /// \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.SetFromEuler(_rpy); + } + + /// \brief Set the pose from a six tuple consisting of + /// x, y, z, roll, pitch, and yaw. + /// \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.SetFromEuler(math::Vector3(_roll, _pitch, _yaw)); + } + + /// \brief See if a pose is finite (e.g., not nan) + /// \return True if this pose is finite. + 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: IGN_DEPRECATED(7) Pose3 operator+(const Pose3 &_pose) const + { + Pose3 result; + + result.p = this->CoordPositionAdd(_pose); + result.q = this->CoordRotationAdd(_pose.q); + + return result; + } + + /// \brief Addition assignment operator. + /// \param[in] _pose Pose3 to add to this pose. + /// \sa operator+(const Pose3 &_pose) const. + /// \return The resulting pose. + public: IGN_DEPRECATED(7) 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 assignment operator. + /// \param[in] _pose Pose3 to subtract from this one + /// \sa operator-(const Pose3 &_pose) const. + /// \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 this pose is equal to the given pose. + 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 this pose is not equal to the given pose. + 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 + /// \sa operator*(const Pose3 &_pose) const + /// \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) = default; + + /// \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 pose 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. + // \return The inverse 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. This sets the position to zero and the + /// rotation to identify. + public: void Reset() + { + // set the position to zero + this->p.Set(); + this->q = Quaternion::Identity; + } + + /// \brief Rotate the 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 Number of decimal places. + 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 mutuable 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 Equality test with tolerance. + /// \param[in] _p The pose to compare this against. Both the position + /// Vector3 and rotation Quaternion are compared. + /// \param[in] _tol Equality tolerance. + /// \return True if the position and orientation of the poses are equal + /// within the tolerence specified by _tol. + public: bool Equal(const Pose3 &_p, const T &_tol) const + { + return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol); + } + + /// \brief The position + private: Vector3 p; + + /// \brief The rotation + private: Quaternion q; + }; + + namespace detail { + + template constexpr Pose3 gPose3Zero{}; + + } // namespace detail + + template const Pose3 &Pose3::Zero = detail::gPose3Zero; + + /// typedef Pose3 as Pose3d. + typedef Pose3 Pose3d; + + /// typedef Pose3 as Pose3f. + typedef Pose3 Pose3f; + } + } +} +#endif diff --git a/include/gz/math/Quaternion.hh b/include/gz/math/Quaternion.hh new file mode 100644 index 000000000..fb9ad29b8 --- /dev/null +++ b/include/gz/math/Quaternion.hh @@ -0,0 +1,1280 @@ +/* + * 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 gz/math/Quaternion.hh + /// \brief A quaternion class that represents 3D rotations and + /// orientations. Four scalar values, [w,x,y,z], are used represent + /// orientations and rotations. + /// + /// The following two type definitions are provided: + /// + /// * \ref Quaternionf + /// * \ref Quaterniond + /// + /// ## Examples + /// + /// * C++ + /// + /// \snippet examples/quaternion_example.cc complete + /// + /// * Ruby + /// + /// \code{.rb} + /// # Modify the RUBYLIB environment variable to include the ignition math + /// # library install path. For example, if you install to /user: + /// # + /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB + /// # + /// require 'gz/math' + /// + /// q = Ignition::Math::Quaterniond.new + /// printf("A default quaternion has the following values\n"+ + /// "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z()) + /// + /// q = Ignition::Math::Quaterniond.Identity + /// printf("The identity quaternion has the following values\n" + + /// "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z()) + /// + /// q2 = Ignition::Math::Quaterniond.new(0, 0, 3.14) + /// printf("A quaternion initialized from roll=0, pitch=0, and yaw=3.14 " + + /// "has the following values\n" + + /// "\tW=%f X=%f Y=%f Z=%f\n", q2.W(), q2.X(), q2.Y(), q2.Z()) + /// + /// euler = q2.Euler() + /// printf("Getting back the euler angles from the quaternion\n" + + /// "\troll=%f pitch=%f yaw=%f\n", euler.X(), euler.Y(), euler.Z()) + /// + /// \endcode + template + class Quaternion + { + /// \brief A Quaternion initialized to identity. + /// This is equivalent to math::Quaternion(1, 0, 0, 0) + public: static const Quaternion &Identity; + + /// \brief A Quaternion initialized to zero. + /// This is equivalent to math::Quaternion(0, 0, 0, 0) + public: static const Quaternion &Zero; + + /// \brief Default Constructor + public: constexpr Quaternion() + : qw(1), qx(0), qy(0), qz(0) + { + // quaternion not normalized, because that breaks + // Pose::CoordPositionAdd(...) + } + + /// \brief Constructor that initializes each value, [w, x, y, z], of + /// the quaternion. This constructor does not normalize the + /// quaternion. + /// \param[in] _w W param + /// \param[in] _x X param + /// \param[in] _y Y param + /// \param[in] _z Z param + public: constexpr Quaternion(const T &_w, const T &_x, const T &_y, + const T &_z) + : qw(_w), qx(_x), qy(_y), qz(_z) + {} + + /// \brief Construct a Quaternion from Euler angles, in radians. This + /// constructor normalizes the quaternion. + /// \param[in] _roll Roll radians. + /// \param[in] _pitch Pitch radians. + /// \param[in] _yaw Yaw radians. + /// \sa SetFromEuler(T, T, T) + public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw) + { + this->SetFromEuler(Vector3(_roll, _pitch, _yaw)); + } + + /// \brief Constructor from an axis and angle. This constructor + /// normalizes the quaternion. + /// \param[in] _axis The rotation axis. + /// \param[in] _angle The rotation angle in radians. + public: Quaternion(const Vector3 &_axis, const T &_angle) + { + this->SetFromAxisAngle(_axis, _angle); + } + + /// \brief Construct a Quaternion from Euler angles, in radians. This + /// constructor normalizes the quaternion. + /// \param[in] _rpy Euler angles in radians. + public: explicit Quaternion(const Vector3 &_rpy) + { + this->SetFromEuler(_rpy); + } + + /// \brief Construct from rotation matrix. This constructor does not + /// normalize the quaternion. + /// \param[in] _mat Rotation matrix (must be orthogonal, the function + /// doesn't check it) + public: explicit Quaternion(const Matrix3 &_mat) + { + this->SetFromMatrix(_mat); + } + + /// \brief Copy constructor. This constructor does not normalize the + /// quaternion. + /// \param[in] _qt Quaternion to copy + public: Quaternion(const Quaternion &_qt) = default; + + /// \brief Destructor + public: ~Quaternion() = default; + + /// \brief Assignment operator + /// \param[in] _qt Quaternion to copy + public: Quaternion &operator=(const Quaternion &_qt) = default; + + /// \brief Invert the quaternion. The quaternion is first normalized, + /// then inverted. + 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 + /// + /// 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. + /// + /// \return The log. + public: Quaternion Log() const + { + 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. + /// + /// 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. + /// + /// \return The exponent. + public: Quaternion Exp() const + { + 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 + /// \deprecated Use SetFromAxisAngle(T, T, T, T) + public: void IGN_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa) + { + this->SetFromAxisAngle(_ax, _ay, _az, _aa); + } + + /// \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 SetFromAxisAngle(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 + /// \deprecated Use SetFromAxisAngle(const Vector3 &_axis, T _a) + public: void IGN_DEPRECATED(7) Axis(const Vector3 &_axis, T _a) + { + this->SetFromAxisAngle(_axis, _a); + } + + /// \brief Set the quaternion from an axis and angle + /// \param[in] _axis Axis + /// \param[in] _a Angle in radians + public: void SetFromAxisAngle(const Vector3 &_axis, T _a) + { + this->SetFromAxisAngle(_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 + /// \deprecated Use SetFromEuler(const Vector3 &) + public: void IGN_DEPRECATED(7) Euler(const Vector3 &_vec) + { + this->SetFromEuler(_vec); + } + + /// \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 angles in radians. + public: void SetFromEuler(const Vector3 &_vec) + { + this->SetFromEuler(_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). + /// \deprecated Use SetFromEuler(T, T, T) + public: void IGN_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw) + { + this->SetFromEuler(_roll, _pitch, _yaw); + } + + /// \brief Set the quaternion from Euler angles. + /// \param[in] _roll Roll angle in radians. + /// \param[in] _pitch Pitch angle in radians. + /// \param[in] _yaw Yaw angle in radians. + public: void SetFromEuler(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, in radians. + /// \return This quaternion as Euler angles. + 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 a quaternion. + /// \param[in] _vec The vector of angles, in radians, to convert. + /// \return The resulting quaternion + public: static Quaternion EulerToQuaternion(const Vector3 &_vec) + { + Quaternion result; + result.SetFromEuler(_vec); + return result; + } + + /// \brief Convert Euler angles, in radians, to a quaternion. + /// \param[in] _x rotation along x in radians + /// \param[in] _y rotation along y in radians + /// \param[in] _z rotation along z in radians + /// \return The resulting 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 + /// \deprecated Use AxisAngle(Vector3 &_axis, T &_angle) const + public: void IGN_DEPRECATED(7) ToAxis(Vector3 &_axis, T &_angle) const + { + this->AxisAngle(_axis, _angle); + } + + /// \brief Convert this quaternion to an axis and angle. + /// \param[out] _axis Rotation axis. + /// \param[out] _angle CCW angle in radians. + public: void AxisAngle(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/ + /// \deprecated Use SetFromMatrix(const Matrix3&) + public: void IGN_DEPRECATED(7) Matrix(const Matrix3 &_mat) + { + this->SetFromMatrix(_mat); + } + + /// \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/ + public: void SetFromMatrix(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 + /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) + public: void IGN_DEPRECATED(7) From2Axes( + const Vector3 &_v1, const Vector3 &_v2) + { + this->SetFrom2Axes(_v1, _v2); + } + + /// \brief Set 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 SetFrom2Axes(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 this quaternion. + /// \param[in] _scale Amount to scale this quaternion + public: void Scale(T _scale) + { + Vector3 axis; + T angle; + + // Convert to axis-and-angle + this->AxisAngle(axis, angle); + angle *= _scale; + + this->SetFromAxisAngle(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 set 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 set 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 set 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 comparison operator. A tolerance of 0.001 is used + /// with the ignition::math::equal function for each component of the + /// quaternions. + /// \param[in] _qt Quaternion for comparison. + /// \return True if each component of both quaternions is within the + /// tolerance of 0.001 of its counterpart. + public: bool operator==(const Quaternion &_qt) const + { + return this->Equal(_qt, static_cast(0.001)); + } + + /// \brief Not equal to operator. A tolerance of 0.001 is used + /// with the ignition::math::equal function for each component of the + /// quaternions. + /// \param[in] _qt Quaternion for comparison. + /// \return True if any component of both quaternions is not within + /// the tolerance of 0.001 of its counterpart. + public: bool operator!=(const Quaternion &_qt) const + { + return !(*this == _qt); + } + + /// \brief Unary minus operator. + /// \return Negation of each component of this 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 Get 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 Get the dot product of this quaternion with the give _q + /// quaternion. + /// \param[in] _q The other quaternion. + /// \return The dot 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 T W() const + { + return this->qw; + } + + /// \brief Get the x component. + /// \return The x quaternion component. + public: inline T X() const + { + return this->qx; + } + + /// \brief Get the y component. + /// \return The y quaternion component. + public: inline T Y() const + { + return this->qy; + } + + /// \brief Get the z component. + /// \return The z quaternion component. + public: inline 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. + /// \deprecated Use SetX(T) + public: inline void IGN_DEPRECATED(7) X(T _v) + { + this->SetX(_v); + } + + /// \brief Set the x component. + /// \param[in] _v The new value for the x quaternion component. + public: inline void SetX(T _v) + { + this->qx = _v; + } + + /// \brief Set the y component. + /// \param[in] _v The new value for the y quaternion component. + /// \deprecated Use SetY(T) + public: inline void IGN_DEPRECATED(7) Y(T _v) + { + this->SetY(_v); + } + + /// \brief Set the y component. + /// \param[in] _v The new value for the y quaternion component. + public: inline void SetY(T _v) + { + this->qy = _v; + } + + + /// \brief Set the z component. + /// \param[in] _v The new value for the z quaternion component. + /// \deprecated Use SetZ(T) + public: inline void IGN_DEPRECATED(7) Z(T _v) + { + this->SetZ(_v); + } + + /// \brief Set the z component. + /// \param[in] _v The new value for the z quaternion component. + public: inline void SetZ(T _v) + { + this->qz = _v; + } + + /// \brief Set the w component. + /// \param[in] _v The new value for the w quaternion component. + /// \deprecated Use SetW(T) + public: inline void IGN_DEPRECATED(7) W(T _v) + { + this->SetW(_v); + } + + /// \brief Set the w component. + /// \param[in] _v The new value for the w quaternion component. + public: inline void SetW(T _v) + { + this->qw = _v; + } + + /// \brief Stream insertion operator + /// \param[in, out] _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 << v; + return _out; + } + + /// \brief Stream extraction operator + /// \param[in, out] _in input stream + /// \param[out] _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.SetFromEuler(Vector3(*roll, *pitch, *yaw)); + } + + return _in; + } + + /// \brief Equality comparison test with a tolerance parameter. + /// The tolerance is used with the ignition::math::equal function for + /// each component of the quaternions. + /// \param[in] _q The quaternion to compare against. + /// \param[in] _tol equality tolerance. + /// \return True if the elements of the quaternions are equal within + /// the tolerence specified by _tol. + public: bool Equal(const Quaternion &_q, const T &_tol) const + { + return equal(this->qx, _q.qx, _tol) && + equal(this->qy, _q.qy, _tol) && + equal(this->qz, _q.qz, _tol) && + equal(this->qw, _q.qw, _tol); + } + + /// \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; + }; + + namespace detail { + + template constexpr Quaternion + gQuaternionIdentity(1, 0, 0, 0); + + template constexpr Quaternion + gQuaternionZero(0, 0, 0, 0); + + } // namespace detail + + template const Quaternion + &Quaternion::Identity = detail::gQuaternionIdentity; + + template const Quaternion + &Quaternion::Zero = detail::gQuaternionZero; + + /// typedef Quaternion as Quaterniond + typedef Quaternion Quaterniond; + + /// typedef Quaternion as Quaternionf + typedef Quaternion Quaternionf; + } + } +} +#endif diff --git a/include/gz/math/Rand.hh b/include/gz/math/Rand.hh new file mode 100644 index 000000000..cf0898259 --- /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 gz/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..1847b2df9 --- /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 gz/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/gz/math/RollingMean.hh b/include/gz/math/RollingMean.hh new file mode 100644 index 000000000..a4744396b --- /dev/null +++ b/include/gz/math/RollingMean.hh @@ -0,0 +1,74 @@ +/* + * 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 { + /// \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 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; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/gz/math/RotationSpline.hh b/include/gz/math/RotationSpline.hh new file mode 100644 index 000000000..f92fa3603 --- /dev/null +++ b/include/gz/math/RotationSpline.hh @@ -0,0 +1,120 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \class RotationSpline RotationSpline.hh gz/math/RotationSpline.hh + /// \brief Spline for rotations + class IGNITION_MATH_VISIBLE RotationSpline + { + /// \brief Constructor. Sets the autoCalc to true + 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 + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/gz/math/SemanticVersion.hh b/include/gz/math/SemanticVersion.hh new file mode 100644 index 000000000..393223a13 --- /dev/null +++ b/include/gz/math/SemanticVersion.hh @@ -0,0 +1,139 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \class SemanticVersion SemanticVersion.hh + /// gz/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 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: explicit SemanticVersion(const unsigned int _major, + const unsigned int _minor = 0, + const unsigned int _patch = 0, + const std::string &_prerelease = "", + const std::string &_build = ""); + + /// \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; + } + + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/SignalStats.hh b/include/gz/math/SignalStats.hh new file mode 100644 index 000000000..0bd6903c7 --- /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 gz/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 gz/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 gz/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 gz/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 gz/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 + /// gz/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 gz/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 gz/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..bd2fba583 --- /dev/null +++ b/include/gz/math/Sphere.hh @@ -0,0 +1,156 @@ +/* + * 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 gz/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 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 "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..fa3481000 --- /dev/null +++ b/include/gz/math/SphericalCoordinates.hh @@ -0,0 +1,241 @@ +/* + * 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 { + + /// \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 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 Pointer to the private data + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Spline.hh b/include/gz/math/Spline.hh new file mode 100644 index 000000000..dc3cb057d --- /dev/null +++ b/include/gz/math/Spline.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. + * +*/ +// 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declare private classes + class ControlPoint; + + /// \class Spline Spline.hh gz/math/Spline.hh + /// \brief Splines + class IGNITION_MATH_VISIBLE Spline + { + /// \brief constructor + 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 + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Stopwatch.hh b/include/gz/math/Stopwatch.hh new file mode 100644 index 000000000..75289aa8e --- /dev/null +++ b/include/gz/math/Stopwatch.hh @@ -0,0 +1,122 @@ +/* + * 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 { + /// \class Stopwatch Stopwatch.hh gz/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 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 Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Temperature.hh b/include/gz/math/Temperature.hh new file mode 100644 index 000000000..e5c53ed6c --- /dev/null +++ b/include/gz/math/Temperature.hh @@ -0,0 +1,370 @@ +/* + * 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" +#include + + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \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. + /// + /// ## Examples + /// + /// * C++ + /// \snippet examples/temperature_example.cc complete + /// + /// * Ruby + /// \code{.cc} + /// # Modify the RUBYLIB environment variable to include the ignition math + /// # library install path. For example, if you install to /user: + /// # + /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB + /// # + /// require 'gz/math' + /// + /// celsius = Ignition::Math::Temperature::KelvinToCelsius(2.5); + /// printf("2.5Kelvin to Celsius is %f\n", celsius) + /// + /// temp = Ignition::Math::Temperature.new(123.5) + /// printf("Constructed a Temperature object with %f Kelvin\n", + /// temp.Kelvin()) + /// + /// printf("Same temperature in Celsius %f\n", temp.Celsius()) + /// + /// temp += 100.0 + /// printf("Temperature + 100.0 is %fK", temp.Kelvin()) + /// + /// newTemp = Ignition::Math::Temperature.new(temp.Kelvin()) + /// newTemp += temp + 23.5; + /// printf("Copied temp and added 23.5K. The new tempurature is %fF\n", + /// newTemp.Fahrenheit()); + /// \endcode + class IGNITION_MATH_VISIBLE Temperature + { + /// \brief Default constructor. + public: Temperature(); + + /// \brief Kelvin value constructor. This is a conversion constructor, + /// and assumes the passed in value is in Kelvin. + /// \param[in] _temp Temperature in Kelvin. + // cppcheck-suppress noExplicitConstructor + public: Temperature(double _temp); + + /// \brief Convert Kelvin to Celsius. + /// \param[in] _temp Temperature in Kelvin. + /// \return Temperature in Celsius. + public: static double KelvinToCelsius(double _temp); + + /// \brief Convert Kelvin to Fahrenheit. + /// \param[in] _temp Temperature in Kelvin. + /// \return Temperature in Fahrenheit. + public: static double KelvinToFahrenheit(double _temp); + + /// \brief Convert Celsius to Fahrenheit. + /// \param[in] _temp Temperature in Celsius. + /// \return Temperature in Fahrenheit. + public: static double CelsiusToFahrenheit(double _temp); + + /// \brief Convert Celsius to Kelvin + /// \param[in] _temp Temperature in Celsius + /// \return Temperature in Kelvin + public: static double CelsiusToKelvin(double _temp); + + /// \brief Convert Fahrenheit to Celsius + /// \param[in] _temp Temperature in Fahrenheit + /// \return Temperature in Celsius + public: static double FahrenheitToCelsius(double _temp); + + /// \brief Convert Fahrenheit to Kelvin + /// \param[in] _temp Temperature in Fahrenheit + /// \return Temperature in Kelvin + public: static double FahrenheitToKelvin(double _temp); + + /// \brief Set the temperature from a Kelvin value. + /// \param[in] _temp Temperature in Kelvin. + public: void SetKelvin(double _temp); + + /// \brief Set the temperature from a Celsius value. + /// \param[in] _temp Temperature in Celsius. + public: void SetCelsius(double _temp); + + /// \brief Set the temperature from a Fahrenheit value. + /// \param[in] _temp Temperature in Fahrenheit. + public: void SetFahrenheit(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=(double _temp); + + /// \brief Addition operator. + /// \param[in] _temp Temperature in Kelvin. + /// \return Resulting temperature. + public: Temperature operator+(double _temp) const; + + /// \brief Addition operator. + /// \param[in] _temp Temperature object. + /// \return Resulting temperature. + public: Temperature operator+(const Temperature &_temp) const; + + /// \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+=(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-(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-=(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*(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*=(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/(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/=(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==(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!=(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<(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<=(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>(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>=(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; + } + + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/math/Triangle.hh b/include/gz/math/Triangle.hh new file mode 100644 index 000000000..b9ff2c138 --- /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 gz/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..6e5013b4e --- /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 gz/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..4c445ea21 --- /dev/null +++ b/include/gz/math/Vector2.hh @@ -0,0 +1,607 @@ +/* + * 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 + +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Vector2 Vector2.hh gz/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: constexpr Vector2() + : data{0, 0} + { + } + + /// \brief Constructor + /// \param[in] _x value along x + /// \param[in] _y value along y + public: constexpr Vector2(const T &_x, const T &_y) + : data{_x, _y} + { + } + + /// \brief Copy constructor + /// \param[in] _v the value + public: Vector2(const Vector2 &_v) = default; + + /// \brief Destructor + public: ~Vector2() = default; + + /// \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) = default; + + /// \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[out] _out output stream + /// \param[in] _pt Vector2 to output + /// \return The stream + public: friend std::ostream + &operator<<(std::ostream &_out, const Vector2 &_pt) + { + for (auto i : {0, 1}) + { + if (i > 0) + _out << " "; + + appendToStream(_out, _pt[i]); + } + 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]; + }; + + namespace detail { + + template + constexpr Vector2 gVector2Zero(0, 0); + + template + constexpr Vector2 gVector2One(1, 1); + + template + constexpr Vector2 gVector2NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + + } // namespace detail + + template + const Vector2 &Vector2::Zero = detail::gVector2Zero; + + template + const Vector2 &Vector2::One = detail::gVector2One; + + template + const Vector2 &Vector2::NaN = detail::gVector2NaN; + + 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..0ed433696 --- /dev/null +++ b/include/gz/math/Vector3.hh @@ -0,0 +1,805 @@ +/* + * 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 gz/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: constexpr Vector3() + : data{0, 0, 0} + { + } + + /// \brief Constructor + /// \param[in] _x value along x + /// \param[in] _y value along y + /// \param[in] _z value along z + public: constexpr Vector3(const T &_x, const T &_y, const T &_z) + : data{_x, _y, _z} + { + } + + /// \brief Copy constructor + /// \param[in] _v a vector + public: Vector3(const Vector3 &_v) = default; + + /// \brief Destructor + public: ~Vector3() = default; + + /// \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 Get the number with the maximum absolute value in the vector + /// \return the element with maximum absolute value + public: T MaxAbs() const + { + T max = std::max(std::abs(this->data[0]), std::abs(this->data[1])); + max = std::max(max, std::abs(this->data[2])); + return max; + } + + /// \brief Get the number with the maximum absolute value in the vector + /// \return the element with minimum absolute value + public: T MinAbs() const + { + T min = std::min(std::abs(this->data[0]), std::abs(this->data[1])); + min = std::min(min, std::abs(this->data[2])); + return min; + } + + /// \brief Assignment operator + /// \param[in] _v a new value + /// \return this + public: Vector3 &operator=(const Vector3 &_v) = default; + + /// \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) + { + for (auto i : {0, 1, 2}) + { + if (i > 0) + _out << " "; + + appendToStream(_out, _pt[i]); + } + + 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]; + }; + + namespace detail { + + template + constexpr Vector3 gVector3Zero(0, 0, 0); + template + constexpr Vector3 gVector3One(1, 1, 1); + template + constexpr Vector3 gVector3UnitX(1, 0, 0); + template + constexpr Vector3 gVector3UnitY(0, 1, 0); + template + constexpr Vector3 gVector3UnitZ(0, 0, 1); + template + constexpr Vector3 gVector3NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + } // namespace detail + + template + const Vector3 &Vector3::Zero = detail::gVector3Zero; + template + const Vector3 &Vector3::One = detail::gVector3One; + template + const Vector3 &Vector3::UnitX = detail::gVector3UnitX; + template + const Vector3 &Vector3::UnitY = detail::gVector3UnitY; + template + const Vector3 &Vector3::UnitZ = detail::gVector3UnitZ; + template + const Vector3 &Vector3::NaN = detail::gVector3NaN; + + 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..90f2da5a4 --- /dev/null +++ b/include/gz/math/Vector3Stats.hh @@ -0,0 +1,106 @@ +/* + * 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 +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + /// \class Vector3Stats Vector3Stats.hh gz/math/Vector3Stats.hh + /// \brief Collection of statistics for a Vector3 signal. + class IGNITION_MATH_VISIBLE Vector3Stats + { + /// \brief Constructor + 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. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif + diff --git a/include/gz/math/Vector4.hh b/include/gz/math/Vector4.hh new file mode 100644 index 000000000..a4c251569 --- /dev/null +++ b/include/gz/math/Vector4.hh @@ -0,0 +1,759 @@ +/* + * 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 gz/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: constexpr Vector4() + : data{0, 0, 0, 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: constexpr Vector4(const T &_x, const T &_y, const T &_z, + const T &_w) + : data{_x, _y, _z, _w} + { + } + + /// \brief Copy constructor + /// \param[in] _v vector + public: Vector4(const Vector4 &_v) = default; + + /// \brief Destructor + public: ~Vector4() = default; + + /// \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) = default; + + /// \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) + { + for (auto i : {0, 1, 2, 3}) + { + if (i > 0) + _out << " "; + + appendToStream(_out, _pt[i]); + } + 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]; + }; + + namespace detail { + + template + constexpr Vector4 gVector4Zero(0, 0, 0, 0); + + template + constexpr Vector4 gVector4One(1, 1, 1, 1); + + template + constexpr Vector4 gVector4NaN( + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()); + + } // namespace detail + + template + const Vector4 &Vector4::Zero = detail::gVector4Zero; + + template + const Vector4 &Vector4::One = detail::gVector4One; + + template + const Vector4 &Vector4::NaN = detail::gVector4NaN; + + 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 100% rename from include/ignition/math/config.hh.in rename to include/gz/math/config.hh.in diff --git a/include/gz/math/detail/Box.hh b/include/gz/math/detail/Box.hh new file mode 100644 index 000000000..f5e1ab082 --- /dev/null +++ b/include/gz/math/detail/Box.hh @@ -0,0 +1,363 @@ +/* + * 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_DETAIL_BOX_HH_ +#define GZ_MATH_DETAIL_BOX_HH_ + +#include "gz/math/Triangle3.hh" + +#include +#include +#include +#include + +namespace ignition +{ +namespace math +{ +////////////////////////////////////////////////// +template +Box::Box(T _length, T _width, T _height) +{ + this->size.X(_length); + this->size.Y(_width); + this->size.Z(_height); +} + +////////////////////////////////////////////////// +template +Box::Box(T _length, T _width, T _height, + const ignition::math::Material &_mat) +{ + this->size.X(_length); + this->size.Y(_width); + this->size.Z(_height); + this->material = _mat; +} + +////////////////////////////////////////////////// +template +Box::Box(const Vector3 &_size) +{ + this->size = _size; +} + +////////////////////////////////////////////////// +template +Box::Box(const Vector3 &_size, const ignition::math::Material &_mat) +{ + this->size = _size; + this->material = _mat; +} + +////////////////////////////////////////////////// +template +math::Vector3 Box::Size() const +{ + return this->size; +} + +////////////////////////////////////////////////// +template +void Box::SetSize(T _length, T _width, T _height) +{ + this->size.X(_length); + this->size.Y(_width); + this->size.Z(_height); +} + +////////////////////////////////////////////////// +template +void Box::SetSize(const math::Vector3 &_size) +{ + this->size = _size; +} + +////////////////////////////////////////////////// +template +const ignition::math::Material &Box::Material() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Box::SetMaterial(const ignition::math::Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Box::operator==(const Box &_b) const +{ + return this->size == _b.size && this->material == _b.material; +} + +////////////////////////////////////////////////// +template +bool Box::operator!=(const Box &_b) const +{ + return !(*this == _b); +} + +///////////////////////////////////////////////// +template +T Box::Volume() const +{ + return this->size.X() * this->size.Y() * this->size.Z(); +} + +////////////////////////////////////////////////// +/// \brief Given a *convex* polygon described by the verices in a given plane, +/// compute the list of triangles which form this polygon. +/// \param[in] _plane The plane in which the vertices exist. +/// \param[in] _vertices The vertices of the polygon. +/// \return A vector of triangles and their sign, or an empty vector +/// if _vertices in the _plane are less than 3. The sign will be +1 if the +/// triangle is outward facing, -1 otherwise. +template +std::vector, T>> TrianglesInPlane( + const Plane &_plane, IntersectionPoints &_vertices) +{ + std::vector, T>> triangles; + std::vector> pointsInPlane; + + Vector3 centroid; + for (const auto &pt : _vertices) + { + if (_plane.Side(pt) == Plane::NO_SIDE) + { + pointsInPlane.push_back(pt); + centroid += pt; + } + } + centroid /= T(pointsInPlane.size()); + + if (pointsInPlane.size() < 3) + return {}; + + // Choose a basis in the plane of the triangle + auto axis1 = (pointsInPlane[0] - centroid).Normalize(); + auto axis2 = axis1.Cross(_plane.Normal()).Normalize(); + + // Since the polygon is always convex, we can try to create a fan of triangles + // by sorting the points by their angle in the plane basis. + std::sort(pointsInPlane.begin(), pointsInPlane.end(), + [centroid, axis1, axis2] (const Vector3 &_a, const Vector3 &_b) + { + auto aDisplacement = _a - centroid; + auto bDisplacement = _b - centroid; + + // project line onto the new basis vectors + // The axis length will never be zero as we have three different points + // and the centroid will be different. + auto aX = axis1.Dot(aDisplacement) / axis1.Length(); + auto aY = axis2.Dot(aDisplacement) / axis2.Length(); + + auto bX = axis1.Dot(bDisplacement) / axis1.Length(); + auto bY = axis2.Dot(bDisplacement) / axis2.Length(); + + return atan2(aY, aX) < atan2(bY, bX); + }); + for (std::size_t i = 0; i < pointsInPlane.size(); ++i) + { + triangles.emplace_back( + Triangle3(pointsInPlane[i], + pointsInPlane[(i + 1) % pointsInPlane.size()], centroid), + (_plane.Side({0, 0, 0}) == Plane::POSITIVE_SIDE) ? -1 : 1); + } + + return triangles; +} + +///////////////////////////////////////////////// +template +T Box::VolumeBelow(const Plane &_plane) const +{ + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.empty()) + return 0; + + auto intersections = this->Intersections(_plane); + // TODO(arjo): investigate the use of _epsilon tolerance as this method + // implicitly uses Vector3::operator==() + verticesBelow.merge(intersections); + + // Reconstruct the cut-box as a triangle mesh by attempting to fit planes. + std::vector, T>> triangles; + + std::vector> planes + { + Plane{Vector3{0, 0, 1}, this->Size().Z()/2}, + Plane{Vector3{0, 0, -1}, this->Size().Z()/2}, + Plane{Vector3{1, 0, 0}, this->Size().X()/2}, + Plane{Vector3{-1, 0, 0}, this->Size().X()/2}, + Plane{Vector3{0, 1, 0}, this->Size().Y()/2}, + Plane{Vector3{0, -1, 0}, this->Size().Y()/2}, + _plane + }; + + for (const auto &p : planes) + { + auto newTriangles = TrianglesInPlane(p, verticesBelow); + triangles.insert(triangles.end(), + newTriangles.begin(), + newTriangles.end()); + } + + // Calculate the volume of the triangles + // https://n-e-r-v-o-u-s.com/blog/?p=4415 + T volume = 0; + for (const auto &triangle : triangles) + { + auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); + auto meshVolume = std::abs(crossProduct.Dot(triangle.first[0])); + volume += triangle.second * meshVolume; + } + + return std::abs(volume)/6; +} + +///////////////////////////////////////////////// +template +std::optional> + Box::CenterOfVolumeBelow(const Plane &_plane) const +{ + auto verticesBelow = this->VerticesBelow(_plane); + if (verticesBelow.empty()) + return std::nullopt; + + auto intersections = this->Intersections(_plane); + verticesBelow.merge(intersections); + + Vector3 centroid; + for (const auto &v : verticesBelow) + { + centroid += v; + } + + return centroid / static_cast(verticesBelow.size()); +} + +///////////////////////////////////////////////// +template +IntersectionPoints Box::VerticesBelow(const Plane &_plane) const +{ + // Get coordinates of all vertice of box + // TODO(arjo): Cache this for performance + IntersectionPoints vertices + { + Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} + }; + + IntersectionPoints verticesBelow; + for (const auto &v : vertices) + { + if (_plane.Distance(v) <= 0) + { + verticesBelow.insert(v); + } + } + + return verticesBelow; +} + +///////////////////////////////////////////////// +template +T Box::DensityFromMass(const T _mass) const +{ + if (this->size.Min() <= 0|| _mass <= 0) + return -1.0; + + return _mass / this->Volume(); +} + +///////////////////////////////////////////////// +template +bool Box::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; +} + +///////////////////////////////////////////////// +template +bool Box::MassMatrix(MassMatrix3 &_massMat) const +{ + return _massMat.SetFromBox(this->material, this->size); +} + + +////////////////////////////////////////////////// +template +IntersectionPoints Box::Intersections( + const Plane &_plane) const +{ + IntersectionPoints intersections; + // These are vertices via which we can describe edges. We only need 4 such + // vertices + std::vector > vertices + { + Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, + Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, + Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2} + }; + + // Axes + std::vector> axes + { + Vector3{1, 0, 0}, + Vector3{0, 1, 0}, + Vector3{0, 0, 1} + }; + + // There are 12 edges, which are checked along 3 axes from 4 box corner + // points. + for (auto &v : vertices) + { + for (auto &a : axes) + { + auto intersection = _plane.Intersection(v, a); + if (intersection.has_value() && + intersection->X() >= -this->size.X()/2 && + intersection->X() <= this->size.X()/2 && + intersection->Y() >= -this->size.Y()/2 && + intersection->Y() <= this->size.Y()/2 && + intersection->Z() >= -this->size.Z()/2 && + intersection->Z() <= this->size.Z()/2) + { + intersections.insert(intersection.value()); + } + } + } + + return intersections; +} + +} +} +#endif diff --git a/include/gz/math/detail/Capsule.hh b/include/gz/math/detail/Capsule.hh new file mode 100644 index 000000000..b3cd6d745 --- /dev/null +++ b/include/gz/math/detail/Capsule.hh @@ -0,0 +1,166 @@ +/* + * 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_DETAIL_CAPSULE_HH_ +#define GZ_MATH_DETAIL_CAPSULE_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Capsule::Capsule(const T _length, const T _radius) +{ + this->length = _length; + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +Capsule::Capsule(const T _length, const T _radius, const Material &_mat) +{ + this->length = _length; + this->radius = _radius; + this->material = _mat; +} + +////////////////////////////////////////////////// +template +T Capsule::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Capsule::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +T Capsule::Length() const +{ + return this->length; +} + +////////////////////////////////////////////////// +template +void Capsule::SetLength(const T _length) +{ + this->length = _length; +} + +////////////////////////////////////////////////// +template +const Material &Capsule::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Capsule::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Capsule::operator==(const Capsule &_capsule) const +{ + return equal(this->radius, _capsule.Radius()) && + equal(this->length, _capsule.Length()) && + this->material == _capsule.Mat(); +} + +////////////////////////////////////////////////// +template +std::optional< MassMatrix3 > Capsule::MassMatrix() const +{ + // mass and moment of inertia of cylinder about centroid + MassMatrix3 cylinder; + cylinder.SetFromCylinderZ(this->material, this->length, this->radius); + + // mass and moment of inertia of hemisphere about centroid + const T r2 = this->radius * this->radius; + const T hemisphereMass = this->material.Density() * + 2. / 3. * IGN_PI * r2 * this->radius; + // efunda.com/math/solids/solids_display.cfm?SolidName=Hemisphere + const T ixx = 83. / 320. * hemisphereMass * r2; + const T izz = 2. / 5. * hemisphereMass * r2; + MassMatrix3 hemisphere(hemisphereMass, Vector3(ixx, ixx, izz), + Vector3::Zero);; + + // Distance from centroid of cylinder to centroid of hemisphere, + // since centroid of hemisphere is 3/8 radius from its flat base + const T dz = this->length / 2. + this->radius * 3. / 8.; + Inertial upperCap(hemisphere, Pose3(0, 0, dz, 0, 0, 0)); + Inertial lowerCap(hemisphere, Pose3(0, 0, -dz, 0, 0, 0)); + + // Use Inertial class to add MassMatrix3 objects at different poses + Inertial capsule = + Inertial(cylinder, Pose3::Zero) + upperCap + lowerCap; + + if (!capsule.MassMatrix().IsValid()) + { + return std::nullopt; + } + + return std::make_optional(capsule.MassMatrix()); +} + +////////////////////////////////////////////////// +template +T Capsule::Volume() const +{ + return IGN_PI * std::pow(this->radius, 2) * + (this->length + 4. / 3. * this->radius); +} + +////////////////////////////////////////////////// +template +bool Capsule::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (isnan(newDensity)) + return false; + + this->material.SetDensity(newDensity); + return true; +} + +////////////////////////////////////////////////// +template +T Capsule::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || this->length <=0 || _mass <= 0) + return std::numeric_limits::quiet_NaN(); + + return _mass / this->Volume(); +} + +} +} +#endif diff --git a/include/gz/math/detail/Cylinder.hh b/include/gz/math/detail/Cylinder.hh new file mode 100644 index 000000000..afb141aa5 --- /dev/null +++ b/include/gz/math/detail/Cylinder.hh @@ -0,0 +1,149 @@ +/* + * 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_DETAIL_CYLINDER_HH_ +#define GZ_MATH_DETAIL_CYLINDER_HH_ +namespace ignition +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Cylinder::Cylinder(const T _length, const T _radius, + const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +Cylinder::Cylinder(const T _length, const T _radius, + const Material &_mat, const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->material = _mat; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +T Cylinder::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Cylinder::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +T Cylinder::Length() const +{ + return this->length; +} + +////////////////////////////////////////////////// +template +void Cylinder::SetLength(const T _length) +{ + this->length = _length; +} + +////////////////////////////////////////////////// +template +Quaternion Cylinder::RotationalOffset() const +{ + return this->rotOffset; +} + +////////////////////////////////////////////////// +template +void Cylinder::SetRotationalOffset(const Quaternion &_rotOffset) +{ + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +const Material &Cylinder::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Cylinder::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Cylinder::operator==(const Cylinder &_cylinder) const +{ + return equal(this->radius, _cylinder.Radius()) && + equal(this->length, _cylinder.Length()) && + this->material == _cylinder.Mat(); +} + +////////////////////////////////////////////////// +template +bool Cylinder::MassMatrix(MassMatrix3d &_massMat) const +{ + return _massMat.SetFromCylinderZ( + this->material, this->length, + this->radius, this->rotOffset); +} + +////////////////////////////////////////////////// +template +T Cylinder::Volume() const +{ + return IGN_PI * std::pow(this->radius, 2) * + this->length; +} + +////////////////////////////////////////////////// +template +bool Cylinder::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; +} + +////////////////////////////////////////////////// +template +T Cylinder::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || this->length <=0 || _mass <= 0) + return -1.0; + + return _mass / this->Volume(); +} + +} +} +#endif diff --git a/include/gz/math/detail/Ellipsoid.hh b/include/gz/math/detail/Ellipsoid.hh new file mode 100644 index 000000000..b84522010 --- /dev/null +++ b/include/gz/math/detail/Ellipsoid.hh @@ -0,0 +1,128 @@ +/* + * 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_DETAIL_ELLIPSOID_HH_ +#define GZ_MATH_DETAIL_ELLIPSOID_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Ellipsoid::Ellipsoid(const Vector3 &_radii) : radii(_radii) {} + +////////////////////////////////////////////////// +template +Ellipsoid::Ellipsoid(const Vector3 &_radii, const Material &_mat) +: radii(_radii), material(_mat) {} + +////////////////////////////////////////////////// +template +Vector3 Ellipsoid::Radii() const +{ + return this->radii; +} + +////////////////////////////////////////////////// +template +void Ellipsoid::SetRadii(const Vector3 &_radii) +{ + this->radii = _radii; +} + +////////////////////////////////////////////////// +template +const Material &Ellipsoid::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Ellipsoid::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Ellipsoid::operator==(const Ellipsoid &_ellipsoid) const +{ + return this->radii == _ellipsoid.radii && + this->material == _ellipsoid.material; +} + +////////////////////////////////////////////////// +template +std::optional< MassMatrix3 > Ellipsoid::MassMatrix() const +{ + if (this->radii.X() <= 0 || this->radii.Y() <= 0 || this->radii.Z() <= 0) + return std::nullopt; + + // mass and inertia of ellipsoid taken from + // https://en.wikipedia.org/wiki/Ellipsoid + const T mass = this->material.Density() * this->Volume(); + const T x2 = std::pow(this->radii.X(), 2); + const T y2 = std::pow(this->radii.Y(), 2); + const T z2 = std::pow(this->radii.Z(), 2); + const T ixx = (mass / 5.) * (y2 + z2); + const T iyy = (mass / 5.) * (x2 + z2); + const T izz = (mass / 5.) * (x2 + y2); + return std::make_optional>( + mass, Vector3(ixx, iyy, izz), Vector3::Zero); +} + +////////////////////////////////////////////////// +template +T Ellipsoid::Volume() const +{ + const T kFourThirdsPi = 4. * IGN_PI / 3.; + return kFourThirdsPi * this->radii.X() * this->radii.Y() * this->radii.Z(); +} + +////////////////////////////////////////////////// +template +bool Ellipsoid::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (isnan(newDensity)) + return false; + + this->material.SetDensity(newDensity); + return true; +} + +////////////////////////////////////////////////// +template +T Ellipsoid::DensityFromMass(const T _mass) const +{ + if (this->radii.X() <= 0 || this->radii.Y() <= 0 || this->radii.Z() <=0 + || _mass <= 0) + return std::numeric_limits::quiet_NaN(); + + return _mass / this->Volume(); +} + +} +} +#endif diff --git a/include/gz/math/detail/Sphere.hh b/include/gz/math/detail/Sphere.hh new file mode 100644 index 000000000..91eddf9f1 --- /dev/null +++ b/include/gz/math/detail/Sphere.hh @@ -0,0 +1,173 @@ +/* + * 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_DETAIL_SPHERE_HH_ +#define GZ_MATH_DETAIL_SPHERE_HH_ + +#include "gz/math/Sphere.hh" + +namespace ignition +{ +namespace math +{ +////////////////////////////////////////////////// +template +Sphere::Sphere(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +Sphere::Sphere(const T _radius, const ignition::math::Material &_mat) +{ + this->radius = _radius; + this->material = _mat; +} + +////////////////////////////////////////////////// +template +T Sphere::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Sphere::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +const ignition::math::Material &Sphere::Material() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Sphere::SetMaterial(const ignition::math::Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Sphere::operator==(const Sphere &_sphere) const +{ + return equal(this->radius, _sphere.Radius()) && + this->material == _sphere.Material(); +} + +////////////////////////////////////////////////// +template +bool Sphere::operator!=(const Sphere &_sphere) const +{ + return !(*this == _sphere); +} + +////////////////////////////////////////////////// +template +bool Sphere::MassMatrix(MassMatrix3d &_massMat) const +{ + return _massMat.SetFromSphere(this->material, this->radius); +} + +////////////////////////////////////////////////// +template +T Sphere::Volume() const +{ + return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3); +} + +////////////////////////////////////////////////// +template +T Sphere::VolumeBelow(const Plane &_plane) const +{ + auto r = this->radius; + // get nearest point to center on plane + auto dist = _plane.Distance(Vector3d(0, 0, 0)); + + if (dist < -r) + { + // sphere is below plane. + return Volume(); + } + else if (dist > r) + { + // sphere is completely above plane + return 0.0; + } + + auto h = r - dist; + return IGN_PI * h * h * (3 * r - h) / 3; +} + +////////////////////////////////////////////////// +template +std::optional> + Sphere::CenterOfVolumeBelow(const Plane &_plane) const +{ + auto r = this->radius; + // get nearest point to center on plane + auto dist = _plane.Distance(Vector3d(0, 0, 0)); + + if (dist < -r) + { + // sphere is completely below plane + return Vector3{0, 0, 0}; + } + else if (dist > r) + { + // sphere is completely above plane + return std::nullopt; + } + + // Get the height of the spherical cap + auto h = r - dist; + + // Formula for geometric centorid: + // https://mathworld.wolfram.com/SphericalCap.html + auto numerator = 2 * r - h; + + auto z = 3 * numerator * numerator / (4 * (3 * r - h)); + return - z * _plane.Normal().Normalized(); +} + +////////////////////////////////////////////////// +template +bool Sphere::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; +} + +////////////////////////////////////////////////// +template +T Sphere::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || _mass <= 0) + return -1.0; + + return _mass / this->Volume(); +} +} +} +#endif diff --git a/include/gz/math/detail/WellOrderedVector.hh b/include/gz/math/detail/WellOrderedVector.hh new file mode 100644 index 000000000..98aebfa7c --- /dev/null +++ b/include/gz/math/detail/WellOrderedVector.hh @@ -0,0 +1,63 @@ +/* + * 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_DETAIL_WELLORDERED_VECTOR_HH_ +#define GZ_MATH_DETAIL_WELLORDERED_VECTOR_HH_ +#include + +namespace ignition +{ + namespace math + { + /// \brief Comparator for well-ordering vectors. + template + struct WellOrderedVectors + { + /// \brief The normal Vector3::operator< is not actually properly ordered. + /// It does not form an ordinal set. This leads to various complications. + /// To solve this we introduce this function which orders vector3's by + /// their X value first, then their Y values and lastly their Z-values. + /// \param[in] _a - first vector + /// \param[in] _b - second vector + /// \return true if _a comes before _b. + bool operator() (const Vector3& _a, const Vector3& _b) const + { + if (_a[0] < _b[0]) + { + return true; + } + else if (equal(_a[0], _b[0], 1e-3)) + { + if (_a[1] < _b[1]) + { + return true; + } + else if (equal(_a[1], _b[1], 1e-3)) + { + return _a[2] < _b[2]; + } + else + { + return false; + } + } + return false; + } + }; + } +} + +#endif diff --git a/include/gz/math/graph/Edge.hh b/include/gz/math/graph/Edge.hh new file mode 100644 index 000000000..80931af0d --- /dev/null +++ b/include/gz/math/graph/Edge.hh @@ -0,0 +1,342 @@ +/* + * 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. + // cppcheck-suppress noExplicitConstructor + 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(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, + 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, + 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..4c0d07905 --- /dev/null +++ b/include/gz/math/graph/Graph.hh @@ -0,0 +1,810 @@ +/* + * 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 +#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. + /// 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/gz/math/graph/GraphAlgorithms.hh b/include/gz/math/graph/GraphAlgorithms.hh new file mode 100644 index 000000000..bbde04fdf --- /dev/null +++ b/include/gz/math/graph/GraphAlgorithms.hh @@ -0,0 +1,360 @@ +/* + * 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()) + { + // cppcheck-suppress useStlAlgorithm + 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..0b2f53899 --- /dev/null +++ b/include/gz/math/graph/Vertex.hh @@ -0,0 +1,152 @@ +/* + * 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. + // cppcheck-suppress noExplicitConstructor + 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..baadd4d4e --- /dev/null +++ b/include/ignition/math.hh @@ -0,0 +1,18 @@ +/* + * 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 diff --git a/include/ignition/math/AdditivelySeparableScalarField3.hh b/include/ignition/math/AdditivelySeparableScalarField3.hh index e05128a35..c080ea3fc 100644 --- a/include/ignition/math/AdditivelySeparableScalarField3.hh +++ b/include/ignition/math/AdditivelySeparableScalarField3.hh @@ -13,185 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Angle.hh b/include/ignition/math/Angle.hh index 5395a4dae..c040942ac 100644 --- a/include/ignition/math/Angle.hh +++ b/include/ignition/math/Angle.hh @@ -13,235 +13,6 @@ * 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 Π - - /// \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() = default; - - /// \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: constexpr Angle(double _radian) - : value(_radian) - { - } - - /// \brief Set the value from an angle in radians. - /// \param[in] _radian Radian value. - /// \deprecated Use void SetRadian(double) - public: void IGN_DEPRECATED(7) 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 - /// \deprecated Use void SetDegree(double) - public: void IGN_DEPRECATED(7) 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, with a default value of zero. - private: double value{0}; - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/AxisAlignedBox.hh b/include/ignition/math/AxisAlignedBox.hh index 706fb4f21..82b2dc1af 100644 --- a/include/ignition/math/AxisAlignedBox.hh +++ b/include/ignition/math/AxisAlignedBox.hh @@ -13,237 +13,6 @@ * 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 { - /// \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 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 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 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 - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Box.hh b/include/ignition/math/Box.hh index 312715627..9b916e378 100644 --- a/include/ignition/math/Box.hh +++ b/include/ignition/math/Box.hh @@ -13,210 +13,6 @@ * 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 "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 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 +#include diff --git a/include/ignition/math/Capsule.hh b/include/ignition/math/Capsule.hh index cbf4ae198..0faa66672 100644 --- a/include/ignition/math/Capsule.hh +++ b/include/ignition/math/Capsule.hh @@ -13,139 +13,6 @@ * 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 diff --git a/include/ignition/math/Color.hh b/include/ignition/math/Color.hh index 79f17a2e4..a362200de 100644 --- a/include/ignition/math/Color.hh +++ b/include/ignition/math/Color.hh @@ -13,380 +13,6 @@ * 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() = default; - - /// \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: constexpr Color(const float _r, const float _g, const float _b, - const float _a = 1.0) - : r(_r), g(_g), b(_b), a(_a) - { - this->Clamp(); - } - - /// \brief Copy Constructor - /// \param[in] _clr Color to copy - public: Color(const Color &_clr) = default; - - /// \brief Destructor - public: ~Color() = default; - - /// \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) = default; - - /// \brief Array index operator - /// \param[in] _index Color component index(0=red, 1=green, 2=blue, - /// 3=alpha) - /// \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 Array index operator, const version - /// \param[in] _index Color component index(0=red, 1=green, 2=blue, - /// 3=alpha) - /// \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) const; - - /// \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: constexpr void Clamp() - { - // These comparisons are carefully written to handle NaNs correctly. - if (!(this->r >= 0)) { this->r = 0; } - if (!(this->g >= 0)) { this->g = 0; } - if (!(this->b >= 0)) { this->b = 0; } - if (!(this->a >= 0)) { this->a = 0; } - if (this->r > 1) { this->r = this->r/255.0f; } - if (this->g > 1) { this->g = this->g/255.0f; } - if (this->b > 1) { this->b = this->b/255.0f; } - if (this->a > 1) { this->a = 1; } - } - - /// \brief Stream insertion operator - /// \param[in] _out the output stream - /// \param[in] _color the color - /// \return the output stream - public: friend std::ostream &operator<<(std::ostream &_out, - const Color &_color) - { - for (auto i : {0, 1, 2, 3}) - { - if (i > 0) - _out << " "; - - appendToStream(_out, _color[i]); - } - 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 +#include diff --git a/include/ignition/math/Cylinder.hh b/include/ignition/math/Cylinder.hh index 93fee1a07..04264af84 100644 --- a/include/ignition/math/Cylinder.hh +++ b/include/ignition/math/Cylinder.hh @@ -13,168 +13,6 @@ * 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 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 diff --git a/include/ignition/math/DiffDriveOdometry.hh b/include/ignition/math/DiffDriveOdometry.hh index 6d62b630d..65d76e4a6 100644 --- a/include/ignition/math/DiffDriveOdometry.hh +++ b/include/ignition/math/DiffDriveOdometry.hh @@ -13,128 +13,6 @@ * 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 - -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 { - /** \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 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); - - /// \brief Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/Ellipsoid.hh b/include/ignition/math/Ellipsoid.hh index 4d67fa9ba..4078fd4b9 100644 --- a/include/ignition/math/Ellipsoid.hh +++ b/include/ignition/math/Ellipsoid.hh @@ -13,122 +13,6 @@ * 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 diff --git a/include/ignition/math/Export.hh b/include/ignition/math/Export.hh new file mode 100644 index 000000000..4b45141c1 --- /dev/null +++ b/include/ignition/math/Export.hh @@ -0,0 +1,18 @@ +/* + * 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 diff --git a/include/ignition/math/Filter.hh b/include/ignition/math/Filter.hh index f88d3c8d9..0029af7c0 100644 --- a/include/ignition/math/Filter.hh +++ b/include/ignition/math/Filter.hh @@ -13,240 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Frustum.hh b/include/ignition/math/Frustum.hh index 0376d2fdd..d2467cbf0 100644 --- a/include/ignition/math/Frustum.hh +++ b/include/ignition/math/Frustum.hh @@ -13,160 +13,6 @@ * 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 { - - /// \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(double _near, - double _far, - const math::Angle &_fov, - double _aspectRatio, - const math::Pose3d &_pose = math::Pose3d::Zero); - - /// \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(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(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(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 Compute the planes of the frustum. This is called whenever - /// a property of the frustum is changed. - private: void ComputePlanes(); - - /// \brief Private data pointer - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/GaussMarkovProcess.hh b/include/ignition/math/GaussMarkovProcess.hh index c048309fe..f411fd60e 100644 --- a/include/ignition/math/GaussMarkovProcess.hh +++ b/include/ignition/math/GaussMarkovProcess.hh @@ -13,125 +13,6 @@ * 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 - -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 { - /** \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 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); - - /// \brief Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Helpers.hh b/include/ignition/math/Helpers.hh index 322e79666..e48efb669 100644 --- a/include/ignition/math/Helpers.hh +++ b/include/ignition/math/Helpers.hh @@ -13,856 +13,6 @@ * 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 "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 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(); - - /// \brief Simple clamping function that constrains a value to - /// a range defined by a min and max value. This function is equivalent to - /// std::max(std::min(value, max), min). - /// \param[in] _v Value to clamp - /// \param[in] _min Minimum allowed value. - /// \param[in] _max Maximum allowed value. - /// \return The value _v clamped to the range defined by _min and _max. - 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 to check. - /// \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 to check - /// \return True if _v is not a number, false otherwise. - inline bool isnan(double _v) - { - return (std::isnan)(_v); - } - - /// \brief Fix a float NaN value. - /// \param[in] _v Value to correct. - /// \return 0 if _v is NaN or infinite, _v otherwise. - inline float fixnan(float _v) - { - return isnan(_v) || std::isinf(_v) ? 0.0f : _v; - } - - /// \brief Fix a double NaN value. - /// \param[in] _v Value to correct. - /// \return 0 if _v is NaN or is infinite, _v otherwise. - inline double fixnan(double _v) - { - return isnan(_v) || std::isinf(_v) ? 0.0 : _v; - } - - /// \brief Check if an int 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 an unsigned int 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 an int 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 an unsigned int 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 value in a vector of values - /// \param[in] _values The vector of values. - /// \return The mean value in the provided vector. - 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 the variance of a vector of values. - /// \param[in] _values The vector of values. - /// \return The squared deviation of the vector of values. - 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 value in the vector. - 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 value in the vector. - 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 - /// \return True if the two values fall within the given 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 Less than or near test, within a tolerance. - /// \param[in] _a The first value. - /// \param[in] _b The second value. - /// \param[in] _epsilon The tolerance. - /// \return True if _a < _b + _tol. - template - inline bool lessOrNearEqual(const T &_a, const T &_b, - const T &_epsilon = 1e-6) - { - return _a < _b + _epsilon; - } - - /// \brief Greater than or near test, within a tolerance. - /// \param[in] _a The first value. - /// \param[in] _b The second value. - /// \param[in] _epsilon The tolerance. - /// \return True if _a > _b - _epsilon. - template - inline bool greaterOrNearEqual(const T &_a, const T &_b, - const T &_epsilon = 1e-6) - { - return _a > _b - _epsilon; - } - - /// \brief Get the 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[in, out] _a The first number. This variable will contain the - /// lower of the two values after this function completes. - /// \param[in, out] _b The second number. This variable will contain the - /// higher of the two values after this function completes. - 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[in,out] _a The first number. This variable will contain the - /// lowest of the three values after this function completes. - /// \param[in,out] _b The second number. This variable will contain the - /// middle of the three values after this function completes. - /// \param[in,out] _c The third number. This variable will contain the - /// highest of the three values after this function completes. - 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. - /// \param[in] _precision Precision for floating point numbers. - /// \deprecated Use appendToStream(std::ostream, T) instead. - template - inline void IGN_DEPRECATED(7) appendToStream( - std::ostream &_out, T _number, int _precision) - { - if (std::fpclassify(_number) == FP_ZERO) - { - _out << 0; - } - else - { - _out << precision(_number, _precision); - } - } - - /// \brief Append a number to a stream, specialized for int. - /// \param[out] _out Output stream. - /// \param[in] _number Number to append. - /// _precision Not used for int. - /// \deprecated Use appendToStream(std::ostream, int) instead. - template<> - inline void IGN_DEPRECATED(7) appendToStream( - std::ostream &_out, int _number, int) - { - _out << _number; - } - - /// \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 the parameter a power of 2? - /// \param[in] _x The number to check. - /// \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 than or equal to - /// a given value. - /// \param[in] _x The value which marks the lower bound of the result. - /// \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 input string. - /// \return An integer, or NAN_I if unable to parse the input. - 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 [in] _input The string. - /// \return A floating point number (can be NaN) or NAN_D if the - /// _input could not be parsed. - 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 - std::string IGNITION_MATH_VISIBLE timePointToString( - const std::chrono::steady_clock::time_point &_point); - - /// \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 - std::string IGNITION_MATH_VISIBLE durationToString( - const std::chrono::steady_clock::duration &_duration); - - /// \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; - - // 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 - - // `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 - bool IGNITION_MATH_VISIBLE splitTimeBasedOnTimeRegex( - const std::string &_timeString, - uint64_t & numberDays, uint64_t & numberHours, - uint64_t & numberMinutes, uint64_t & numberSeconds, - uint64_t & numberMilliseconds); - - /// \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. - std::chrono::steady_clock::duration IGNITION_MATH_VISIBLE stringToDuration( - const std::string &_timeString); - - /// \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. - std::chrono::steady_clock::time_point - IGNITION_MATH_VISIBLE stringToTimePoint(const std::string &_timeString); - - // 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 implementation 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 +#include diff --git a/include/ignition/math/Inertial.hh b/include/ignition/math/Inertial.hh index 00a133ff1..252f824e0 100644 --- a/include/ignition/math/Inertial.hh +++ b/include/ignition/math/Inertial.hh @@ -13,257 +13,6 @@ * 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 "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) = default; - - /// \brief Destructor. - public: ~Inertial() = default; - - /// \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; - } - - /// \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) = default; - - /// \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 +#include diff --git a/include/ignition/math/Interval.hh b/include/ignition/math/Interval.hh index 115db95dc..f4a0a8bed 100644 --- a/include/ignition/math/Interval.hh +++ b/include/ignition/math/Interval.hh @@ -13,287 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Kmeans.hh b/include/ignition/math/Kmeans.hh index 585685edc..82fea2de7 100644 --- a/include/ignition/math/Kmeans.hh +++ b/include/ignition/math/Kmeans.hh @@ -13,73 +13,6 @@ * 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 { - - /// \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 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; - - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/Line2.hh b/include/ignition/math/Line2.hh index 0b669f868..ea74c5939 100644 --- a/include/ignition/math/Line2.hh +++ b/include/ignition/math/Line2.hh @@ -13,308 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Line3.hh b/include/ignition/math/Line3.hh index 55b7ceb90..99fea8a78 100644 --- a/include/ignition/math/Line3.hh +++ b/include/ignition/math/Line3.hh @@ -13,402 +13,6 @@ * 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 - -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) = default; - - /// \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) = default; - - /// \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 +#include diff --git a/include/ignition/math/MassMatrix3.hh b/include/ignition/math/MassMatrix3.hh index d99c1a5f7..307f4783f 100644 --- a/include/ignition/math/MassMatrix3.hh +++ b/include/ignition/math/MassMatrix3.hh @@ -13,1092 +13,6 @@ * 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 "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) = default; - - /// \brief Destructor. - public: ~MassMatrix3() = default; - - /// \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. - 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. - 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. - public: bool SetOffDiagonalMoments(const Vector3 &_ixyxzyz) - { - this->Ixyxzyz = _ixyxzyz; - return this->IsValid(); - } - - /// \brief Get IXX - /// \return IXX value - public: T Ixx() const - { - return this->Ixxyyzz[0]; - } - - /// \brief Get IYY - /// \return IYY value - public: T Iyy() const - { - return this->Ixxyyzz[1]; - } - - /// \brief Get IZZ - /// \return IZZ value - public: T Izz() const - { - return this->Ixxyyzz[2]; - } - - /// \brief Get IXY - /// \return IXY value - public: T Ixy() const - { - return this->Ixyxzyz[0]; - } - - /// \brief Get IXZ - /// \return IXZ value - public: T Ixz() const - { - return this->Ixyxzyz[1]; - } - - /// \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. - 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. - 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. - 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. - 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. - 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. - 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 - 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. - 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) - = default; - - /// \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 - { - // Compute tolerance relative to maximum value of inertia diagonal - T tol = _tol * this->Ixxyyzz.Max(); - Vector3 moments = this->PrincipalMoments(tol); - 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 +#include diff --git a/include/ignition/math/Material.hh b/include/ignition/math/Material.hh index 061baeef2..4628a81bc 100644 --- a/include/ignition/math/Material.hh +++ b/include/ignition/math/Material.hh @@ -13,130 +13,6 @@ * 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 { - - /// \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 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 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. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/MaterialType.hh b/include/ignition/math/MaterialType.hh index 72f36496f..0eedd3083 100644 --- a/include/ignition/math/MaterialType.hh +++ b/include/ignition/math/MaterialType.hh @@ -13,87 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MATERIALTYPE_HH_ -#define IGNITION_MATH_MATERIALTYPE_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 +#include diff --git a/include/ignition/math/Matrix3.hh b/include/ignition/math/Matrix3.hh index 1580352da..36f1c98c5 100644 --- a/include/ignition/math/Matrix3.hh +++ b/include/ignition/math/Matrix3.hh @@ -13,663 +13,6 @@ * 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 - -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. - /// - /// The following two type definitions are provided: - /// - /// * \ref Matrix3i : Equivalent to Matrix3 - /// * \ref Matrix3f : Equivalent to Matrix3 - /// * \ref Matrix3d : Equivalent to Matrix3 - /// ## Examples - /// - /// * C++ - /// - /// \snippet examples/matrix3_example.cc complete - /// - /// * Ruby - /// \code{.rb} - /// # Modify the RUBYLIB environment variable to include the ignition math - /// # library install path. For example, if you install to /user: - /// # - /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB - /// # - /// require 'ignition/math' - /// - /// # Construct a default matrix3. - /// m = Ignition::Math::Matrix3d.new - /// printf("The default constructed matrix m has the following "+ - /// "values.\n\t" + - /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", - /// m.(0, 0), m.(0, 1), m.(0, 2), - /// m.(1, 0), m.(1, 1), m.(1, 2), - /// m.(2, 0), m.(2, 1), m.(2, 2)) - /// - /// # Set the first column of the matrix. - /// m.SetCol(0, Ignition::Math::Vector3d.new(3, 4, 5)) - /// printf("Setting the first column of the matrix m to 3, 4, 5.\n\t" + - /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", - /// m.(0, 0), m.(0, 1), m.(0, 2), - /// m.(1, 0), m.(1, 1), m.(1, 2), - /// m.(2, 0), m.(2, 1), m.(2, 2)) - /// - /// # Transpose the matrix. - /// t = m.Transposed() - /// printf("The transposed matrix t has the values.\n\t"+ - /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", - /// t.(0, 0), t.(0, 1), t.(0, 2), - /// t.(1, 0), t.(1, 1), t.(1, 2), - /// t.(2, 0), t.(2, 1), t.(2, 2)) - /// - /// # Multiply the two matrices. - /// m = m * t - /// printf("m * t = " + - /// "%2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f %2.1f\n", - /// m.(0, 0), m.(0, 1), m.(0, 2), - /// m.(1, 0), m.(1, 1), m.(1, 2), - /// m.(2, 0), m.(2, 1), m.(2, 2)) - /// \endcode - template - class Matrix3 - { - /// \brief A Matrix3 initialized to identity. - /// This is equivalent to math::Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1). - public: static const Matrix3 &Identity; - - /// \brief A Matrix3 initialized to zero. - /// This is equivalent to math::Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0). - public: static const Matrix3 &Zero; - - /// \brief Default constructor that initializes the matrix3 to zero. - public: Matrix3() - { - std::memset(this->data, 0, sizeof(this->data[0][0])*9); - } - - /// \brief Copy constructor. - /// \param _m Matrix to copy - public: Matrix3(const Matrix3 &_m) = default; - - /// \brief Construct a matrix3 using nine values. - /// \param[in] _v00 Row 0, Col 0 value - /// \param[in] _v01 Row 0, Col 1 value - /// \param[in] _v02 Row 0, Col 2 value - /// \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: constexpr Matrix3(T _v00, T _v01, T _v02, - T _v10, T _v11, T _v12, - T _v20, T _v21, T _v22) - : data{{_v00, _v01, _v02}, - {_v10, _v11, _v12}, - {_v20, _v21, _v22}} - { - } - - /// \brief Construct 3x3 rotation Matrix from a quaternion. - /// \param[in] _q Quaternion to set the Matrix3 from. - 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: ~Matrix3() = default; - - /// \brief Set a single value. - /// \param[in] _row row index. _row is clamped to the range [0,2] - /// \param[in] _col column index. _col is clamped to the range [0,2] - /// \param[in] _v New value. - public: void Set(size_t _row, size_t _col, T _v) - { - this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] - [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)] = _v; - } - - /// \brief Set values. - /// \param[in] _v00 Row 0, Col 0 value - /// \param[in] _v01 Row 0, Col 1 value - /// \param[in] _v02 Row 0, Col 2 value - /// \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, the first column of the matrix. - /// \param[in] _yAxis The y axis, the second column of the matrix. - /// \param[in] _zAxis The z axis, the third column of the matrix. - /// \deprecated Use SetAxes(const Vector3 &, const Vector3 &, - /// const Vector3 &,) - public: void IGN_DEPRECATED(7) Axes(const Vector3 &_xAxis, - const Vector3 &_yAxis, - const Vector3 &_zAxis) - { - this->SetAxes(_xAxis, _yAxis, _zAxis); - } - - /// \brief Set the matrix from three axis (1 per column). - /// \param[in] _xAxis The x axis, the first column of the matrix. - /// \param[in] _yAxis The y axis, the second column of the matrix. - /// \param[in] _zAxis The z axis, the third column of the matrix. - public: void SetAxes(const Vector3 &_xAxis, - const Vector3 &_yAxis, - const Vector3 &_zAxis) - { - this->SetCol(0, _xAxis); - this->SetCol(1, _yAxis); - this->SetCol(2, _zAxis); - } - - /// \brief Set as a rotation matrix from an axis and angle. - /// \param[in] _axis the axis - /// \param[in] _angle ccw rotation around the axis in radians - /// \deprecated Use SetFromAxisAngle(const Vector3 &, T) - public: void IGN_DEPRECATED(7) Axis(const Vector3 &_axis, T _angle) - { - this->SetFromAxisAngle(_axis, _angle); - } - - /// \brief Set as a rotation matrix from an axis and angle. - /// \param[in] _axis the axis - /// \param[in] _angle ccw rotation around the axis in radians - public: void SetFromAxisAngle(const Vector3 &_axis, T _angle) - { - T c = cos(_angle); - T s = sin(_angle); - 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 as a rotation 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 - /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) - public: void IGN_DEPRECATED(7) From2Axes( - const Vector3 &_v1, const Vector3 &_v2) - { - this->SetFrom2Axes(_v1, _v2); - } - - /// \brief Set as a rotation matrix to represent rotation from - /// vector _v1 to vector _v2, so that - /// _v2.Normalize() == this * _v1.Normalize() holds. - /// - /// \param[in] _v1 The first vector - /// \param[in] _v2 The second vector - public: void SetFrom2Axes(const Vector3 &_v1, const Vector3 &_v2) - { - const T _v1LengthSquared = _v1.SquaredLength(); - if (_v1LengthSquared <= 0.0) - { - // 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->SetFromAxisAngle(cross, acos(dot)); - } - - /// \brief Set a column. - /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the - /// range [0, 2]. - /// \param[in] _v The value to set in each row of the column. - /// \deprecated Use SetCol(unsigned int _c, const Vector3 &_v) - public: void IGN_DEPRECATED(7) Col(unsigned int _c, const Vector3 &_v) - { - this->SetCol(_c, _v); - } - - /// \brief Set a column. - /// \param[in] _c The colum index [0, 1, 2]. _col is clamped to the - /// range [0, 2]. - /// \param[in] _v The value to set in each row of the column. - public: void SetCol(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 Matrix to copy. - /// \return This matrix. - public: Matrix3 &operator=(const Matrix3 &_mat) = default; - - /// \brief Subtraction operator. - /// \param[in] _m Matrix to subtract. - /// \return The element wise difference of two matrices. - public: Matrix3 operator-(const Matrix3 &_m) const - { - return Matrix3( - 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 Addition operation. - /// \param[in] _m Matrix to add. - /// \return The element wise sum of two matrices - public: Matrix3 operator+(const Matrix3 &_m) const - { - return Matrix3( - 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 Scalar multiplication operator. - /// \param[in] _s Value to multiply. - /// \return 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 as a 3x3 rotation matrix 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 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. This operator outputs all - /// 9 scalar values in the matrix separated by a single space. - /// \param[in, out] _out Output stream. - /// \param[in] _m Matrix to output. - /// \return The stream. - public: friend std::ostream &operator<<( - std::ostream &_out, const ignition::math::Matrix3 &_m) - { - for (auto i : {0, 1, 2}) - { - for (auto j : {0, 1, 2}) - { - if (!(i == 0 && j == 0)) - _out << " "; - - appendToStream(_out, _m(i, j)); - } - } - - return _out; - } - - /// \brief Stream extraction operator. This operator requires 9 space - /// separated scalar values, such as "1 2 3 4 5 6 7 8 9". - /// \param [in, out] _in Input stream. - /// \param [out] _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]; - }; - - namespace detail { - - template - constexpr Matrix3 gMatrix3Identity( - 1, 0, 0, - 0, 1, 0, - 0, 0, 1); - - template - constexpr Matrix3 gMatrix3Zero( - 0, 0, 0, - 0, 0, 0, - 0, 0, 0); - - } // namespace detail - - template - const Matrix3 &Matrix3::Identity = detail::gMatrix3Identity; - - template - const Matrix3 &Matrix3::Zero = detail::gMatrix3Zero; - - /// typedef Matrix3 as Matrix3i. - typedef Matrix3 Matrix3i; - - /// typedef Matrix3 as Matrix3d. - typedef Matrix3 Matrix3d; - - /// typedef Matrix3 as Matrix3f. - typedef Matrix3 Matrix3f; - } - } -} -#endif +#include diff --git a/include/ignition/math/Matrix4.hh b/include/ignition/math/Matrix4.hh index 5feec6b6d..b1636dfc8 100644 --- a/include/ignition/math/Matrix4.hh +++ b/include/ignition/math/Matrix4.hh @@ -13,868 +13,6 @@ * 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 - -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) = 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] _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: constexpr 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) - : data{{_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: ~Matrix4() = default; - - /// \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 - 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. - 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.SetW(root / 2.0); - root = 1.0 / (2.0 * root); - q.SetX((this->data[2][1] - this->data[1][2]) * root); - q.SetY((this->data[0][2] - this->data[2][0]) * root); - q.SetZ((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.SetX(a); break; - case 1: q.SetY(a); break; - case 2: q.SetZ(a); break; - }; - switch (j) - { - default: - case 0: q.SetX(b); break; - case 1: q.SetY(b); break; - case 2: q.SetZ(b); break; - }; - switch (k) - { - default: - case 0: q.SetX(c); break; - case 1: q.SetY(c); break; - case 2: q.SetZ(c); break; - }; - - q.SetW((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 - /// \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) = default; - - /// \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) - { - for (auto i : {0, 1, 2, 3}) - { - for (auto j : {0, 1, 2, 3}) - { - 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 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]; - }; - - namespace detail { - - template - constexpr Matrix4 gMatrix4Identity( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1); - - template - constexpr Matrix4 gMatrix4Zero( - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0); - - } // namespace detail - - template - const Matrix4 &Matrix4::Identity = detail::gMatrix4Identity; - - template - const Matrix4 &Matrix4::Zero = detail::gMatrix4Zero; - - typedef Matrix4 Matrix4i; - typedef Matrix4 Matrix4d; - typedef Matrix4 Matrix4f; - } - } -} -#endif +#include diff --git a/include/ignition/math/MovingWindowFilter.hh b/include/ignition/math/MovingWindowFilter.hh index 67bb20c94..a2ee0370b 100644 --- a/include/ignition/math/MovingWindowFilter.hh +++ b/include/ignition/math/MovingWindowFilter.hh @@ -13,178 +13,6 @@ * 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 diff --git a/include/ignition/math/OrientedBox.hh b/include/ignition/math/OrientedBox.hh index a835e8faf..b62a7bef9 100644 --- a/include/ignition/math/OrientedBox.hh +++ b/include/ignition/math/OrientedBox.hh @@ -13,262 +13,6 @@ * 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 - -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) = default; - - /// \brief Destructor - public: ~OrientedBox() = default; - - /// \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(const Vector3 &_size) - { - // Enforce non-negative size - this->size = _size.Abs(); - } - - /// \brief Set the box pose. - /// \param[in] _pose Box pose. - public: void Pose(const 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) = default; - - /// \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 OrientedBoxd; - typedef OrientedBox OrientedBoxf; - } - } -} -#endif +#include diff --git a/include/ignition/math/PID.hh b/include/ignition/math/PID.hh index fa80d9a37..c1ad844cf 100644 --- a/include/ignition/math/PID.hh +++ b/include/ignition/math/PID.hh @@ -13,179 +13,6 @@ * 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 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 Reset the errors and command. - public: void Reset(); - - /// \brief Pointer to private data. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/PiecewiseScalarField3.hh b/include/ignition/math/PiecewiseScalarField3.hh index ce938d1bf..a8bb2ba09 100644 --- a/include/ignition/math/PiecewiseScalarField3.hh +++ b/include/ignition/math/PiecewiseScalarField3.hh @@ -13,207 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Plane.hh b/include/ignition/math/Plane.hh index 30dab92e0..eb0ad995c 100644 --- a/include/ignition/math/Plane.hh +++ b/include/ignition/math/Plane.hh @@ -13,270 +13,6 @@ * 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 - -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: explicit 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) = default; - - /// \brief Destructor - public: ~Plane() = default; - - /// \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) = default; - - /// \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 +#include diff --git a/include/ignition/math/Polynomial3.hh b/include/ignition/math/Polynomial3.hh index 4112d8f80..9b01782c7 100644 --- a/include/ignition/math/Polynomial3.hh +++ b/include/ignition/math/Polynomial3.hh @@ -13,283 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index 8e6eb14fd..c3a8d6071 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -13,548 +13,6 @@ * 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 - -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 The Pose3 class represents a 3D position and rotation. The - /// position component is a Vector3, and the rotation is a Quaternion. - /// - /// The following two type definitions are provided: - /// - /// * \ref Pose3f - /// * \ref Pose3d - /// ## Examples - /// - /// * C++ - /// - /// \snippet examples/pose3_example.cc complete - /// - /// * Ruby - /// - /// \code{.rb} - /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB - /// # - /// require 'ignition/math' - /// - /// # Construct a default Pose3d. - /// p = Ignition::Math::Pose3d.new - /// printf("A default Pose3d has the following values\n" + - /// "%f %f %f %f %f %f\n", p.Pos().X(), p.Pos().Y(), p.Pos().Z(), - /// p.Rot().Euler().X(), p.Rot().Euler().Y(), p.Rot().Euler().Z()) - /// - /// # Construct a pose at position 1, 2, 3 with a yaw of PI radians. - /// p1 = Ignition::Math::Pose3d.new(1, 2, 3, 0, 0, Math::PI) - /// printf("A pose3d(1, 2, 3, 0, 0, IGN_PI) has the following values\n" + - /// "%f %f %f %f %f %f\n", p1.Pos().X(), p1.Pos().Y(), p1.Pos().Z(), - /// p1.Rot().Euler().X(), p1.Rot().Euler().Y(), p1.Rot().Euler().Z()) - /// - /// # Set the position of a pose to 10, 20, 30 - /// p.Pos().Set(10, 20, 30) - /// - /// p3 = p * p1 - /// printf("Result of combining two poses is\n"+ - /// "%f %f %f %f %f %f\n", p3.Pos().X(), p3.Pos().Y(), p3.Pos().Z(), - /// p3.Rot().Euler().X(), p3.Rot().Euler().Y(), p3.Rot().Euler().Z()) - /// \endcode - template - class Pose3 - { - /// \brief A Pose3 initialized to zero. - /// This is equivalent to math::Pose3(0, 0, 0, 0, 0, 0). - public: static const Pose3 &Zero; - - /// \brief Default constructor. This initializes the position - /// component to zero and the quaternion to identity. - public: Pose3() = default; - - /// \brief Create a Pose3 based on a position and rotation. - /// \param[in] _pos The position - /// \param[in] _rot The rotation - public: Pose3(const Vector3 &_pos, const Quaternion &_rot) - : p(_pos), q(_rot) - { - } - - /// \brief Create a Pose3 using a 6-tuple consisting of - /// x, y, z, roll, pitch, and yaw. - /// \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 Create a Pose3 using a 7-tuple consisting of - /// x, y, z, qw, qx, qy, qz. The first three values are the position - /// and the last four the rotation represented as a quaternion. - /// \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) = default; - - /// \brief Destructor. - public: ~Pose3() = default; - - /// \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 a position and Euler angles. - /// \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.SetFromEuler(_rpy); - } - - /// \brief Set the pose from a six tuple consisting of - /// x, y, z, roll, pitch, and yaw. - /// \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.SetFromEuler(math::Vector3(_roll, _pitch, _yaw)); - } - - /// \brief See if a pose is finite (e.g., not nan) - /// \return True if this pose is finite. - 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: IGN_DEPRECATED(7) Pose3 operator+(const Pose3 &_pose) const - { - Pose3 result; - - result.p = this->CoordPositionAdd(_pose); - result.q = this->CoordRotationAdd(_pose.q); - - return result; - } - - /// \brief Addition assignment operator. - /// \param[in] _pose Pose3 to add to this pose. - /// \sa operator+(const Pose3 &_pose) const. - /// \return The resulting pose. - public: IGN_DEPRECATED(7) 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 assignment operator. - /// \param[in] _pose Pose3 to subtract from this one - /// \sa operator-(const Pose3 &_pose) const. - /// \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 this pose is equal to the given pose. - 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 this pose is not equal to the given pose. - 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 - /// \sa operator*(const Pose3 &_pose) const - /// \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) = default; - - /// \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 pose 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. - // \return The inverse 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. This sets the position to zero and the - /// rotation to identify. - public: void Reset() - { - // set the position to zero - this->p.Set(); - this->q = Quaternion::Identity; - } - - /// \brief Rotate the 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 Number of decimal places. - 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 mutuable 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 Equality test with tolerance. - /// \param[in] _p The pose to compare this against. Both the position - /// Vector3 and rotation Quaternion are compared. - /// \param[in] _tol Equality tolerance. - /// \return True if the position and orientation of the poses are equal - /// within the tolerence specified by _tol. - public: bool Equal(const Pose3 &_p, const T &_tol) const - { - return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol); - } - - /// \brief The position - private: Vector3 p; - - /// \brief The rotation - private: Quaternion q; - }; - - namespace detail { - - template constexpr Pose3 gPose3Zero{}; - - } // namespace detail - - template const Pose3 &Pose3::Zero = detail::gPose3Zero; - - /// typedef Pose3 as Pose3d. - typedef Pose3 Pose3d; - - /// typedef Pose3 as Pose3f. - typedef Pose3 Pose3f; - } - } -} -#endif +#include diff --git a/include/ignition/math/Quaternion.hh b/include/ignition/math/Quaternion.hh index 93bf17d9b..a50e006da 100644 --- a/include/ignition/math/Quaternion.hh +++ b/include/ignition/math/Quaternion.hh @@ -13,1268 +13,6 @@ * 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 - -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 that represents 3D rotations and - /// orientations. Four scalar values, [w,x,y,z], are used represent - /// orientations and rotations. - /// - /// The following two type definitions are provided: - /// - /// * \ref Quaternionf - /// * \ref Quaterniond - /// - /// ## Examples - /// - /// * C++ - /// - /// \snippet examples/quaternion_example.cc complete - /// - /// * Ruby - /// - /// \code{.rb} - /// # Modify the RUBYLIB environment variable to include the ignition math - /// # library install path. For example, if you install to /user: - /// # - /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB - /// # - /// require 'ignition/math' - /// - /// q = Ignition::Math::Quaterniond.new - /// printf("A default quaternion has the following values\n"+ - /// "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z()) - /// - /// q = Ignition::Math::Quaterniond.Identity - /// printf("The identity quaternion has the following values\n" + - /// "\tW=%f X=%f Y=%f Z=%f\n", q.W(), q.X(), q.Y(), q.Z()) - /// - /// q2 = Ignition::Math::Quaterniond.new(0, 0, 3.14) - /// printf("A quaternion initialized from roll=0, pitch=0, and yaw=3.14 " + - /// "has the following values\n" + - /// "\tW=%f X=%f Y=%f Z=%f\n", q2.W(), q2.X(), q2.Y(), q2.Z()) - /// - /// euler = q2.Euler() - /// printf("Getting back the euler angles from the quaternion\n" + - /// "\troll=%f pitch=%f yaw=%f\n", euler.X(), euler.Y(), euler.Z()) - /// - /// \endcode - template - class Quaternion - { - /// \brief A Quaternion initialized to identity. - /// This is equivalent to math::Quaternion(1, 0, 0, 0) - public: static const Quaternion &Identity; - - /// \brief A Quaternion initialized to zero. - /// This is equivalent to math::Quaternion(0, 0, 0, 0) - public: static const Quaternion &Zero; - - /// \brief Default Constructor - public: constexpr Quaternion() - : qw(1), qx(0), qy(0), qz(0) - { - // quaternion not normalized, because that breaks - // Pose::CoordPositionAdd(...) - } - - /// \brief Constructor that initializes each value, [w, x, y, z], of - /// the quaternion. This constructor does not normalize the - /// quaternion. - /// \param[in] _w W param - /// \param[in] _x X param - /// \param[in] _y Y param - /// \param[in] _z Z param - public: constexpr Quaternion(const T &_w, const T &_x, const T &_y, - const T &_z) - : qw(_w), qx(_x), qy(_y), qz(_z) - {} - - /// \brief Construct a Quaternion from Euler angles, in radians. This - /// constructor normalizes the quaternion. - /// \param[in] _roll Roll radians. - /// \param[in] _pitch Pitch radians. - /// \param[in] _yaw Yaw radians. - /// \sa SetFromEuler(T, T, T) - public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw) - { - this->SetFromEuler(Vector3(_roll, _pitch, _yaw)); - } - - /// \brief Constructor from an axis and angle. This constructor - /// normalizes the quaternion. - /// \param[in] _axis The rotation axis. - /// \param[in] _angle The rotation angle in radians. - public: Quaternion(const Vector3 &_axis, const T &_angle) - { - this->SetFromAxisAngle(_axis, _angle); - } - - /// \brief Construct a Quaternion from Euler angles, in radians. This - /// constructor normalizes the quaternion. - /// \param[in] _rpy Euler angles in radians. - public: explicit Quaternion(const Vector3 &_rpy) - { - this->SetFromEuler(_rpy); - } - - /// \brief Construct from rotation matrix. This constructor does not - /// normalize the quaternion. - /// \param[in] _mat Rotation matrix (must be orthogonal, the function - /// doesn't check it) - public: explicit Quaternion(const Matrix3 &_mat) - { - this->SetFromMatrix(_mat); - } - - /// \brief Copy constructor. This constructor does not normalize the - /// quaternion. - /// \param[in] _qt Quaternion to copy - public: Quaternion(const Quaternion &_qt) = default; - - /// \brief Destructor - public: ~Quaternion() = default; - - /// \brief Assignment operator - /// \param[in] _qt Quaternion to copy - public: Quaternion &operator=(const Quaternion &_qt) = default; - - /// \brief Invert the quaternion. The quaternion is first normalized, - /// then inverted. - 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 - /// - /// 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. - /// - /// \return The log. - public: Quaternion Log() const - { - 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. - /// - /// 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. - /// - /// \return The exponent. - public: Quaternion Exp() const - { - 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 - /// \deprecated Use SetFromAxisAngle(T, T, T, T) - public: void IGN_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa) - { - this->SetFromAxisAngle(_ax, _ay, _az, _aa); - } - - /// \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 SetFromAxisAngle(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 - /// \deprecated Use SetFromAxisAngle(const Vector3 &_axis, T _a) - public: void IGN_DEPRECATED(7) Axis(const Vector3 &_axis, T _a) - { - this->SetFromAxisAngle(_axis, _a); - } - - /// \brief Set the quaternion from an axis and angle - /// \param[in] _axis Axis - /// \param[in] _a Angle in radians - public: void SetFromAxisAngle(const Vector3 &_axis, T _a) - { - this->SetFromAxisAngle(_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 - /// \deprecated Use SetFromEuler(const Vector3 &) - public: void IGN_DEPRECATED(7) Euler(const Vector3 &_vec) - { - this->SetFromEuler(_vec); - } - - /// \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 angles in radians. - public: void SetFromEuler(const Vector3 &_vec) - { - this->SetFromEuler(_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). - /// \deprecated Use SetFromEuler(T, T, T) - public: void IGN_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw) - { - this->SetFromEuler(_roll, _pitch, _yaw); - } - - /// \brief Set the quaternion from Euler angles. - /// \param[in] _roll Roll angle in radians. - /// \param[in] _pitch Pitch angle in radians. - /// \param[in] _yaw Yaw angle in radians. - public: void SetFromEuler(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, in radians. - /// \return This quaternion as Euler angles. - 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 a quaternion. - /// \param[in] _vec The vector of angles, in radians, to convert. - /// \return The resulting quaternion - public: static Quaternion EulerToQuaternion(const Vector3 &_vec) - { - Quaternion result; - result.SetFromEuler(_vec); - return result; - } - - /// \brief Convert Euler angles, in radians, to a quaternion. - /// \param[in] _x rotation along x in radians - /// \param[in] _y rotation along y in radians - /// \param[in] _z rotation along z in radians - /// \return The resulting 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 - /// \deprecated Use AxisAngle(Vector3 &_axis, T &_angle) const - public: void IGN_DEPRECATED(7) ToAxis(Vector3 &_axis, T &_angle) const - { - this->AxisAngle(_axis, _angle); - } - - /// \brief Convert this quaternion to an axis and angle. - /// \param[out] _axis Rotation axis. - /// \param[out] _angle CCW angle in radians. - public: void AxisAngle(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/ - /// \deprecated Use SetFromMatrix(const Matrix3&) - public: void IGN_DEPRECATED(7) Matrix(const Matrix3 &_mat) - { - this->SetFromMatrix(_mat); - } - - /// \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/ - public: void SetFromMatrix(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 - /// \deprecated Use SetFrom2Axes(const Vector3 &, const Vector3 &) - public: void IGN_DEPRECATED(7) From2Axes( - const Vector3 &_v1, const Vector3 &_v2) - { - this->SetFrom2Axes(_v1, _v2); - } - - /// \brief Set 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 SetFrom2Axes(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 this quaternion. - /// \param[in] _scale Amount to scale this quaternion - public: void Scale(T _scale) - { - Vector3 axis; - T angle; - - // Convert to axis-and-angle - this->AxisAngle(axis, angle); - angle *= _scale; - - this->SetFromAxisAngle(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 set 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 set 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 set 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 comparison operator. A tolerance of 0.001 is used - /// with the ignition::math::equal function for each component of the - /// quaternions. - /// \param[in] _qt Quaternion for comparison. - /// \return True if each component of both quaternions is within the - /// tolerance of 0.001 of its counterpart. - public: bool operator==(const Quaternion &_qt) const - { - return this->Equal(_qt, static_cast(0.001)); - } - - /// \brief Not equal to operator. A tolerance of 0.001 is used - /// with the ignition::math::equal function for each component of the - /// quaternions. - /// \param[in] _qt Quaternion for comparison. - /// \return True if any component of both quaternions is not within - /// the tolerance of 0.001 of its counterpart. - public: bool operator!=(const Quaternion &_qt) const - { - return !(*this == _qt); - } - - /// \brief Unary minus operator. - /// \return Negation of each component of this 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 Get 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 Get the dot product of this quaternion with the give _q - /// quaternion. - /// \param[in] _q The other quaternion. - /// \return The dot 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 T W() const - { - return this->qw; - } - - /// \brief Get the x component. - /// \return The x quaternion component. - public: inline T X() const - { - return this->qx; - } - - /// \brief Get the y component. - /// \return The y quaternion component. - public: inline T Y() const - { - return this->qy; - } - - /// \brief Get the z component. - /// \return The z quaternion component. - public: inline 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. - /// \deprecated Use SetX(T) - public: inline void IGN_DEPRECATED(7) X(T _v) - { - this->SetX(_v); - } - - /// \brief Set the x component. - /// \param[in] _v The new value for the x quaternion component. - public: inline void SetX(T _v) - { - this->qx = _v; - } - - /// \brief Set the y component. - /// \param[in] _v The new value for the y quaternion component. - /// \deprecated Use SetY(T) - public: inline void IGN_DEPRECATED(7) Y(T _v) - { - this->SetY(_v); - } - - /// \brief Set the y component. - /// \param[in] _v The new value for the y quaternion component. - public: inline void SetY(T _v) - { - this->qy = _v; - } - - - /// \brief Set the z component. - /// \param[in] _v The new value for the z quaternion component. - /// \deprecated Use SetZ(T) - public: inline void IGN_DEPRECATED(7) Z(T _v) - { - this->SetZ(_v); - } - - /// \brief Set the z component. - /// \param[in] _v The new value for the z quaternion component. - public: inline void SetZ(T _v) - { - this->qz = _v; - } - - /// \brief Set the w component. - /// \param[in] _v The new value for the w quaternion component. - /// \deprecated Use SetW(T) - public: inline void IGN_DEPRECATED(7) W(T _v) - { - this->SetW(_v); - } - - /// \brief Set the w component. - /// \param[in] _v The new value for the w quaternion component. - public: inline void SetW(T _v) - { - this->qw = _v; - } - - /// \brief Stream insertion operator - /// \param[in, out] _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 << v; - return _out; - } - - /// \brief Stream extraction operator - /// \param[in, out] _in input stream - /// \param[out] _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.SetFromEuler(Vector3(*roll, *pitch, *yaw)); - } - - return _in; - } - - /// \brief Equality comparison test with a tolerance parameter. - /// The tolerance is used with the ignition::math::equal function for - /// each component of the quaternions. - /// \param[in] _q The quaternion to compare against. - /// \param[in] _tol equality tolerance. - /// \return True if the elements of the quaternions are equal within - /// the tolerence specified by _tol. - public: bool Equal(const Quaternion &_q, const T &_tol) const - { - return equal(this->qx, _q.qx, _tol) && - equal(this->qy, _q.qy, _tol) && - equal(this->qz, _q.qz, _tol) && - equal(this->qw, _q.qw, _tol); - } - - /// \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; - }; - - namespace detail { - - template constexpr Quaternion - gQuaternionIdentity(1, 0, 0, 0); - - template constexpr Quaternion - gQuaternionZero(0, 0, 0, 0); - - } // namespace detail - - template const Quaternion - &Quaternion::Identity = detail::gQuaternionIdentity; - - template const Quaternion - &Quaternion::Zero = detail::gQuaternionZero; - - /// typedef Quaternion as Quaterniond - typedef Quaternion Quaterniond; - - /// typedef Quaternion as Quaternionf - typedef Quaternion Quaternionf; - } - } -} -#endif +#include diff --git a/include/ignition/math/Rand.hh b/include/ignition/math/Rand.hh index c1f9a1340..bb1b6d9e2 100644 --- a/include/ignition/math/Rand.hh +++ b/include/ignition/math/Rand.hh @@ -13,79 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Region3.hh b/include/ignition/math/Region3.hh index 339a9d91b..fdb86a12b 100644 --- a/include/ignition/math/Region3.hh +++ b/include/ignition/math/Region3.hh @@ -13,197 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/RollingMean.hh b/include/ignition/math/RollingMean.hh index 6b3f3f96b..c37891bcf 100644 --- a/include/ignition/math/RollingMean.hh +++ b/include/ignition/math/RollingMean.hh @@ -13,62 +13,6 @@ * 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 - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - /// \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 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; - - /// \brief Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/RotationSpline.hh b/include/ignition/math/RotationSpline.hh index 7ca354855..f650a74d8 100644 --- a/include/ignition/math/RotationSpline.hh +++ b/include/ignition/math/RotationSpline.hh @@ -13,108 +13,6 @@ * 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 { - /// \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 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 - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/SemanticVersion.hh b/include/ignition/math/SemanticVersion.hh index 9042aa823..78f50ebb4 100644 --- a/include/ignition/math/SemanticVersion.hh +++ b/include/ignition/math/SemanticVersion.hh @@ -13,127 +13,6 @@ * 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 { - /// \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 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: explicit SemanticVersion(const unsigned int _major, - const unsigned int _minor = 0, - const unsigned int _patch = 0, - const std::string &_prerelease = "", - const std::string &_build = ""); - - /// \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; - } - - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/SignalStats.hh b/include/ignition/math/SignalStats.hh index 2e1924ebe..e4267dbf9 100644 --- a/include/ignition/math/SignalStats.hh +++ b/include/ignition/math/SignalStats.hh @@ -13,243 +13,6 @@ * 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 - -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 + */ +#include diff --git a/include/ignition/math/SpeedLimiter.hh b/include/ignition/math/SpeedLimiter.hh index 0376ff7b4..5430372eb 100644 --- a/include/ignition/math/SpeedLimiter.hh +++ b/include/ignition/math/SpeedLimiter.hh @@ -13,137 +13,6 @@ * 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 "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 +#include diff --git a/include/ignition/math/Sphere.hh b/include/ignition/math/Sphere.hh index 3d62ca9d4..a2b1aa806 100644 --- a/include/ignition/math/Sphere.hh +++ b/include/ignition/math/Sphere.hh @@ -13,144 +13,6 @@ * 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 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 diff --git a/include/ignition/math/SphericalCoordinates.hh b/include/ignition/math/SphericalCoordinates.hh index 5e006fd3e..00c21c993 100644 --- a/include/ignition/math/SphericalCoordinates.hh +++ b/include/ignition/math/SphericalCoordinates.hh @@ -13,229 +13,6 @@ * 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 - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - - /// \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 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 Pointer to the private data - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Spline.hh b/include/ignition/math/Spline.hh index 8c16a8517..1d7f2849e 100644 --- a/include/ignition/math/Spline.hh +++ b/include/ignition/math/Spline.hh @@ -13,251 +13,6 @@ * 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 Spline Spline.hh ignition/math/Spline.hh - /// \brief Splines - class IGNITION_MATH_VISIBLE Spline - { - /// \brief constructor - 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 - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Stopwatch.hh b/include/ignition/math/Stopwatch.hh index f9a12a62e..b87cbc3ef 100644 --- a/include/ignition/math/Stopwatch.hh +++ b/include/ignition/math/Stopwatch.hh @@ -13,110 +13,6 @@ * 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 - -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 { - /// \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 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 Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Temperature.hh b/include/ignition/math/Temperature.hh index 19b0f22dc..9d2ab5403 100644 --- a/include/ignition/math/Temperature.hh +++ b/include/ignition/math/Temperature.hh @@ -13,358 +13,6 @@ * 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 "ignition/math/Helpers.hh" -#include - - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - /// \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. - /// - /// ## Examples - /// - /// * C++ - /// \snippet examples/temperature_example.cc complete - /// - /// * Ruby - /// \code{.cc} - /// # Modify the RUBYLIB environment variable to include the ignition math - /// # library install path. For example, if you install to /user: - /// # - /// # $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB - /// # - /// require 'ignition/math' - /// - /// celsius = Ignition::Math::Temperature::KelvinToCelsius(2.5); - /// printf("2.5Kelvin to Celsius is %f\n", celsius) - /// - /// temp = Ignition::Math::Temperature.new(123.5) - /// printf("Constructed a Temperature object with %f Kelvin\n", - /// temp.Kelvin()) - /// - /// printf("Same temperature in Celsius %f\n", temp.Celsius()) - /// - /// temp += 100.0 - /// printf("Temperature + 100.0 is %fK", temp.Kelvin()) - /// - /// newTemp = Ignition::Math::Temperature.new(temp.Kelvin()) - /// newTemp += temp + 23.5; - /// printf("Copied temp and added 23.5K. The new tempurature is %fF\n", - /// newTemp.Fahrenheit()); - /// \endcode - class IGNITION_MATH_VISIBLE Temperature - { - /// \brief Default constructor. - public: Temperature(); - - /// \brief Kelvin value constructor. This is a conversion constructor, - /// and assumes the passed in value is in Kelvin. - /// \param[in] _temp Temperature in Kelvin. - // cppcheck-suppress noExplicitConstructor - public: Temperature(double _temp); - - /// \brief Convert Kelvin to Celsius. - /// \param[in] _temp Temperature in Kelvin. - /// \return Temperature in Celsius. - public: static double KelvinToCelsius(double _temp); - - /// \brief Convert Kelvin to Fahrenheit. - /// \param[in] _temp Temperature in Kelvin. - /// \return Temperature in Fahrenheit. - public: static double KelvinToFahrenheit(double _temp); - - /// \brief Convert Celsius to Fahrenheit. - /// \param[in] _temp Temperature in Celsius. - /// \return Temperature in Fahrenheit. - public: static double CelsiusToFahrenheit(double _temp); - - /// \brief Convert Celsius to Kelvin - /// \param[in] _temp Temperature in Celsius - /// \return Temperature in Kelvin - public: static double CelsiusToKelvin(double _temp); - - /// \brief Convert Fahrenheit to Celsius - /// \param[in] _temp Temperature in Fahrenheit - /// \return Temperature in Celsius - public: static double FahrenheitToCelsius(double _temp); - - /// \brief Convert Fahrenheit to Kelvin - /// \param[in] _temp Temperature in Fahrenheit - /// \return Temperature in Kelvin - public: static double FahrenheitToKelvin(double _temp); - - /// \brief Set the temperature from a Kelvin value. - /// \param[in] _temp Temperature in Kelvin. - public: void SetKelvin(double _temp); - - /// \brief Set the temperature from a Celsius value. - /// \param[in] _temp Temperature in Celsius. - public: void SetCelsius(double _temp); - - /// \brief Set the temperature from a Fahrenheit value. - /// \param[in] _temp Temperature in Fahrenheit. - public: void SetFahrenheit(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=(double _temp); - - /// \brief Addition operator. - /// \param[in] _temp Temperature in Kelvin. - /// \return Resulting temperature. - public: Temperature operator+(double _temp) const; - - /// \brief Addition operator. - /// \param[in] _temp Temperature object. - /// \return Resulting temperature. - public: Temperature operator+(const Temperature &_temp) const; - - /// \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+=(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-(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-=(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*(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*=(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/(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/=(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==(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!=(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<(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<=(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>(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>=(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; - } - - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/Triangle.hh b/include/ignition/math/Triangle.hh index 6ad394ae0..e06d35e4d 100644 --- a/include/ignition/math/Triangle.hh +++ b/include/ignition/math/Triangle.hh @@ -13,235 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Triangle3.hh b/include/ignition/math/Triangle3.hh index f68d5c187..39b29e2b0 100644 --- a/include/ignition/math/Triangle3.hh +++ b/include/ignition/math/Triangle3.hh @@ -13,274 +13,6 @@ * 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 - -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 +#include diff --git a/include/ignition/math/Vector2.hh b/include/ignition/math/Vector2.hh index 69f7979d4..a477170e6 100644 --- a/include/ignition/math/Vector2.hh +++ b/include/ignition/math/Vector2.hh @@ -13,595 +13,6 @@ * 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 -#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: constexpr Vector2() - : data{0, 0} - { - } - - /// \brief Constructor - /// \param[in] _x value along x - /// \param[in] _y value along y - public: constexpr Vector2(const T &_x, const T &_y) - : data{_x, _y} - { - } - - /// \brief Copy constructor - /// \param[in] _v the value - public: Vector2(const Vector2 &_v) = default; - - /// \brief Destructor - public: ~Vector2() = default; - - /// \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) = default; - - /// \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[out] _out output stream - /// \param[in] _pt Vector2 to output - /// \return The stream - public: friend std::ostream - &operator<<(std::ostream &_out, const Vector2 &_pt) - { - for (auto i : {0, 1}) - { - if (i > 0) - _out << " "; - - appendToStream(_out, _pt[i]); - } - 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]; - }; - - namespace detail { - - template - constexpr Vector2 gVector2Zero(0, 0); - - template - constexpr Vector2 gVector2One(1, 1); - - template - constexpr Vector2 gVector2NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - - } // namespace detail - - template - const Vector2 &Vector2::Zero = detail::gVector2Zero; - - template - const Vector2 &Vector2::One = detail::gVector2One; - - template - const Vector2 &Vector2::NaN = detail::gVector2NaN; - - typedef Vector2 Vector2i; - typedef Vector2 Vector2d; - typedef Vector2 Vector2f; - } - } -} -#endif +#include diff --git a/include/ignition/math/Vector3.hh b/include/ignition/math/Vector3.hh index 5b6c2fc49..6af392588 100644 --- a/include/ignition/math/Vector3.hh +++ b/include/ignition/math/Vector3.hh @@ -13,793 +13,6 @@ * 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 - -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: constexpr Vector3() - : data{0, 0, 0} - { - } - - /// \brief Constructor - /// \param[in] _x value along x - /// \param[in] _y value along y - /// \param[in] _z value along z - public: constexpr Vector3(const T &_x, const T &_y, const T &_z) - : data{_x, _y, _z} - { - } - - /// \brief Copy constructor - /// \param[in] _v a vector - public: Vector3(const Vector3 &_v) = default; - - /// \brief Destructor - public: ~Vector3() = default; - - /// \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 Get the number with the maximum absolute value in the vector - /// \return the element with maximum absolute value - public: T MaxAbs() const - { - T max = std::max(std::abs(this->data[0]), std::abs(this->data[1])); - max = std::max(max, std::abs(this->data[2])); - return max; - } - - /// \brief Get the number with the maximum absolute value in the vector - /// \return the element with minimum absolute value - public: T MinAbs() const - { - T min = std::min(std::abs(this->data[0]), std::abs(this->data[1])); - min = std::min(min, std::abs(this->data[2])); - return min; - } - - /// \brief Assignment operator - /// \param[in] _v a new value - /// \return this - public: Vector3 &operator=(const Vector3 &_v) = default; - - /// \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) - { - for (auto i : {0, 1, 2}) - { - if (i > 0) - _out << " "; - - appendToStream(_out, _pt[i]); - } - - 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]; - }; - - namespace detail { - - template - constexpr Vector3 gVector3Zero(0, 0, 0); - template - constexpr Vector3 gVector3One(1, 1, 1); - template - constexpr Vector3 gVector3UnitX(1, 0, 0); - template - constexpr Vector3 gVector3UnitY(0, 1, 0); - template - constexpr Vector3 gVector3UnitZ(0, 0, 1); - template - constexpr Vector3 gVector3NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - } // namespace detail - - template - const Vector3 &Vector3::Zero = detail::gVector3Zero; - template - const Vector3 &Vector3::One = detail::gVector3One; - template - const Vector3 &Vector3::UnitX = detail::gVector3UnitX; - template - const Vector3 &Vector3::UnitY = detail::gVector3UnitY; - template - const Vector3 &Vector3::UnitZ = detail::gVector3UnitZ; - template - const Vector3 &Vector3::NaN = detail::gVector3NaN; - - typedef Vector3 Vector3i; - typedef Vector3 Vector3d; - typedef Vector3 Vector3f; - } - } -} -#endif +#include diff --git a/include/ignition/math/Vector3Stats.hh b/include/ignition/math/Vector3Stats.hh index 1e85305e8..bc90654f9 100644 --- a/include/ignition/math/Vector3Stats.hh +++ b/include/ignition/math/Vector3Stats.hh @@ -13,94 +13,6 @@ * 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 { - /// \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 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. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif + */ +#include diff --git a/include/ignition/math/Vector4.hh b/include/ignition/math/Vector4.hh index 121e755a2..6f524d279 100644 --- a/include/ignition/math/Vector4.hh +++ b/include/ignition/math/Vector4.hh @@ -13,747 +13,6 @@ * 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 - -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: constexpr Vector4() - : data{0, 0, 0, 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: constexpr Vector4(const T &_x, const T &_y, const T &_z, - const T &_w) - : data{_x, _y, _z, _w} - { - } - - /// \brief Copy constructor - /// \param[in] _v vector - public: Vector4(const Vector4 &_v) = default; - - /// \brief Destructor - public: ~Vector4() = default; - - /// \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) = default; - - /// \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) - { - for (auto i : {0, 1, 2, 3}) - { - if (i > 0) - _out << " "; - - appendToStream(_out, _pt[i]); - } - 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]; - }; - - namespace detail { - - template - constexpr Vector4 gVector4Zero(0, 0, 0, 0); - - template - constexpr Vector4 gVector4One(1, 1, 1, 1); - - template - constexpr Vector4 gVector4NaN( - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()); - - } // namespace detail - - template - const Vector4 &Vector4::Zero = detail::gVector4Zero; - - template - const Vector4 &Vector4::One = detail::gVector4One; - - template - const Vector4 &Vector4::NaN = detail::gVector4NaN; - - typedef Vector4 Vector4i; - typedef Vector4 Vector4d; - typedef Vector4 Vector4f; - } - } -} -#endif +#include diff --git a/include/ignition/math/config.hh b/include/ignition/math/config.hh new file mode 100644 index 000000000..71685131d --- /dev/null +++ b/include/ignition/math/config.hh @@ -0,0 +1,18 @@ +/* + * 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 diff --git a/include/ignition/math/detail/Box.hh b/include/ignition/math/detail/Box.hh index 2ce5f0f9b..7ccd94539 100644 --- a/include/ignition/math/detail/Box.hh +++ b/include/ignition/math/detail/Box.hh @@ -13,351 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_BOX_HH_ -#define IGNITION_MATH_DETAIL_BOX_HH_ + */ -#include "ignition/math/Triangle3.hh" - -#include -#include -#include -#include - -namespace ignition -{ -namespace math -{ -////////////////////////////////////////////////// -template -Box::Box(T _length, T _width, T _height) -{ - this->size.X(_length); - this->size.Y(_width); - this->size.Z(_height); -} - -////////////////////////////////////////////////// -template -Box::Box(T _length, T _width, T _height, - const ignition::math::Material &_mat) -{ - this->size.X(_length); - this->size.Y(_width); - this->size.Z(_height); - this->material = _mat; -} - -////////////////////////////////////////////////// -template -Box::Box(const Vector3 &_size) -{ - this->size = _size; -} - -////////////////////////////////////////////////// -template -Box::Box(const Vector3 &_size, const ignition::math::Material &_mat) -{ - this->size = _size; - this->material = _mat; -} - -////////////////////////////////////////////////// -template -math::Vector3 Box::Size() const -{ - return this->size; -} - -////////////////////////////////////////////////// -template -void Box::SetSize(T _length, T _width, T _height) -{ - this->size.X(_length); - this->size.Y(_width); - this->size.Z(_height); -} - -////////////////////////////////////////////////// -template -void Box::SetSize(const math::Vector3 &_size) -{ - this->size = _size; -} - -////////////////////////////////////////////////// -template -const ignition::math::Material &Box::Material() const -{ - return this->material; -} - -////////////////////////////////////////////////// -template -void Box::SetMaterial(const ignition::math::Material &_mat) -{ - this->material = _mat; -} - -////////////////////////////////////////////////// -template -bool Box::operator==(const Box &_b) const -{ - return this->size == _b.size && this->material == _b.material; -} - -////////////////////////////////////////////////// -template -bool Box::operator!=(const Box &_b) const -{ - return !(*this == _b); -} - -///////////////////////////////////////////////// -template -T Box::Volume() const -{ - return this->size.X() * this->size.Y() * this->size.Z(); -} - -////////////////////////////////////////////////// -/// \brief Given a *convex* polygon described by the verices in a given plane, -/// compute the list of triangles which form this polygon. -/// \param[in] _plane The plane in which the vertices exist. -/// \param[in] _vertices The vertices of the polygon. -/// \return A vector of triangles and their sign, or an empty vector -/// if _vertices in the _plane are less than 3. The sign will be +1 if the -/// triangle is outward facing, -1 otherwise. -template -std::vector, T>> TrianglesInPlane( - const Plane &_plane, IntersectionPoints &_vertices) -{ - std::vector, T>> triangles; - std::vector> pointsInPlane; - - Vector3 centroid; - for (const auto &pt : _vertices) - { - if (_plane.Side(pt) == Plane::NO_SIDE) - { - pointsInPlane.push_back(pt); - centroid += pt; - } - } - centroid /= T(pointsInPlane.size()); - - if (pointsInPlane.size() < 3) - return {}; - - // Choose a basis in the plane of the triangle - auto axis1 = (pointsInPlane[0] - centroid).Normalize(); - auto axis2 = axis1.Cross(_plane.Normal()).Normalize(); - - // Since the polygon is always convex, we can try to create a fan of triangles - // by sorting the points by their angle in the plane basis. - std::sort(pointsInPlane.begin(), pointsInPlane.end(), - [centroid, axis1, axis2] (const Vector3 &_a, const Vector3 &_b) - { - auto aDisplacement = _a - centroid; - auto bDisplacement = _b - centroid; - - // project line onto the new basis vectors - // The axis length will never be zero as we have three different points - // and the centroid will be different. - auto aX = axis1.Dot(aDisplacement) / axis1.Length(); - auto aY = axis2.Dot(aDisplacement) / axis2.Length(); - - auto bX = axis1.Dot(bDisplacement) / axis1.Length(); - auto bY = axis2.Dot(bDisplacement) / axis2.Length(); - - return atan2(aY, aX) < atan2(bY, bX); - }); - for (std::size_t i = 0; i < pointsInPlane.size(); ++i) - { - triangles.emplace_back( - Triangle3(pointsInPlane[i], - pointsInPlane[(i + 1) % pointsInPlane.size()], centroid), - (_plane.Side({0, 0, 0}) == Plane::POSITIVE_SIDE) ? -1 : 1); - } - - return triangles; -} - -///////////////////////////////////////////////// -template -T Box::VolumeBelow(const Plane &_plane) const -{ - auto verticesBelow = this->VerticesBelow(_plane); - if (verticesBelow.empty()) - return 0; - - auto intersections = this->Intersections(_plane); - // TODO(arjo): investigate the use of _epsilon tolerance as this method - // implicitly uses Vector3::operator==() - verticesBelow.merge(intersections); - - // Reconstruct the cut-box as a triangle mesh by attempting to fit planes. - std::vector, T>> triangles; - - std::vector> planes - { - Plane{Vector3{0, 0, 1}, this->Size().Z()/2}, - Plane{Vector3{0, 0, -1}, this->Size().Z()/2}, - Plane{Vector3{1, 0, 0}, this->Size().X()/2}, - Plane{Vector3{-1, 0, 0}, this->Size().X()/2}, - Plane{Vector3{0, 1, 0}, this->Size().Y()/2}, - Plane{Vector3{0, -1, 0}, this->Size().Y()/2}, - _plane - }; - - for (const auto &p : planes) - { - auto newTriangles = TrianglesInPlane(p, verticesBelow); - triangles.insert(triangles.end(), - newTriangles.begin(), - newTriangles.end()); - } - - // Calculate the volume of the triangles - // https://n-e-r-v-o-u-s.com/blog/?p=4415 - T volume = 0; - for (const auto &triangle : triangles) - { - auto crossProduct = (triangle.first[2]).Cross(triangle.first[1]); - auto meshVolume = std::abs(crossProduct.Dot(triangle.first[0])); - volume += triangle.second * meshVolume; - } - - return std::abs(volume)/6; -} - -///////////////////////////////////////////////// -template -std::optional> - Box::CenterOfVolumeBelow(const Plane &_plane) const -{ - auto verticesBelow = this->VerticesBelow(_plane); - if (verticesBelow.empty()) - return std::nullopt; - - auto intersections = this->Intersections(_plane); - verticesBelow.merge(intersections); - - Vector3 centroid; - for (const auto &v : verticesBelow) - { - centroid += v; - } - - return centroid / static_cast(verticesBelow.size()); -} - -///////////////////////////////////////////////// -template -IntersectionPoints Box::VerticesBelow(const Plane &_plane) const -{ - // Get coordinates of all vertice of box - // TODO(arjo): Cache this for performance - IntersectionPoints vertices - { - Vector3{this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, - Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2} - }; - - IntersectionPoints verticesBelow; - for (const auto &v : vertices) - { - if (_plane.Distance(v) <= 0) - { - verticesBelow.insert(v); - } - } - - return verticesBelow; -} - -///////////////////////////////////////////////// -template -T Box::DensityFromMass(const T _mass) const -{ - if (this->size.Min() <= 0|| _mass <= 0) - return -1.0; - - return _mass / this->Volume(); -} - -///////////////////////////////////////////////// -template -bool Box::SetDensityFromMass(const T _mass) -{ - T newDensity = this->DensityFromMass(_mass); - if (newDensity > 0) - this->material.SetDensity(newDensity); - return newDensity > 0; -} - -///////////////////////////////////////////////// -template -bool Box::MassMatrix(MassMatrix3 &_massMat) const -{ - return _massMat.SetFromBox(this->material, this->size); -} - - -////////////////////////////////////////////////// -template -IntersectionPoints Box::Intersections( - const Plane &_plane) const -{ - IntersectionPoints intersections; - // These are vertices via which we can describe edges. We only need 4 such - // vertices - std::vector > vertices - { - Vector3{-this->size.X()/2, -this->size.Y()/2, -this->size.Z()/2}, - Vector3{this->size.X()/2, this->size.Y()/2, -this->size.Z()/2}, - Vector3{this->size.X()/2, -this->size.Y()/2, this->size.Z()/2}, - Vector3{-this->size.X()/2, this->size.Y()/2, this->size.Z()/2} - }; - - // Axes - std::vector> axes - { - Vector3{1, 0, 0}, - Vector3{0, 1, 0}, - Vector3{0, 0, 1} - }; - - // There are 12 edges, which are checked along 3 axes from 4 box corner - // points. - for (auto &v : vertices) - { - for (auto &a : axes) - { - auto intersection = _plane.Intersection(v, a); - if (intersection.has_value() && - intersection->X() >= -this->size.X()/2 && - intersection->X() <= this->size.X()/2 && - intersection->Y() >= -this->size.Y()/2 && - intersection->Y() <= this->size.Y()/2 && - intersection->Z() >= -this->size.Z()/2 && - intersection->Z() <= this->size.Z()/2) - { - intersections.insert(intersection.value()); - } - } - } - - return intersections; -} - -} -} -#endif +#include diff --git a/include/ignition/math/detail/Capsule.hh b/include/ignition/math/detail/Capsule.hh index ebf52b616..e548a21d8 100644 --- a/include/ignition/math/detail/Capsule.hh +++ b/include/ignition/math/detail/Capsule.hh @@ -13,154 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_CAPSULE_HH_ -#define IGNITION_MATH_DETAIL_CAPSULE_HH_ + */ -#include -#include -#include -#include - -namespace ignition -{ -namespace math -{ - -////////////////////////////////////////////////// -template -Capsule::Capsule(const T _length, const T _radius) -{ - this->length = _length; - this->radius = _radius; -} - -////////////////////////////////////////////////// -template -Capsule::Capsule(const T _length, const T _radius, const Material &_mat) -{ - this->length = _length; - this->radius = _radius; - this->material = _mat; -} - -////////////////////////////////////////////////// -template -T Capsule::Radius() const -{ - return this->radius; -} - -////////////////////////////////////////////////// -template -void Capsule::SetRadius(const T _radius) -{ - this->radius = _radius; -} - -////////////////////////////////////////////////// -template -T Capsule::Length() const -{ - return this->length; -} - -////////////////////////////////////////////////// -template -void Capsule::SetLength(const T _length) -{ - this->length = _length; -} - -////////////////////////////////////////////////// -template -const Material &Capsule::Mat() const -{ - return this->material; -} - -////////////////////////////////////////////////// -template -void Capsule::SetMat(const Material &_mat) -{ - this->material = _mat; -} - -////////////////////////////////////////////////// -template -bool Capsule::operator==(const Capsule &_capsule) const -{ - return equal(this->radius, _capsule.Radius()) && - equal(this->length, _capsule.Length()) && - this->material == _capsule.Mat(); -} - -////////////////////////////////////////////////// -template -std::optional< MassMatrix3 > Capsule::MassMatrix() const -{ - // mass and moment of inertia of cylinder about centroid - MassMatrix3 cylinder; - cylinder.SetFromCylinderZ(this->material, this->length, this->radius); - - // mass and moment of inertia of hemisphere about centroid - const T r2 = this->radius * this->radius; - const T hemisphereMass = this->material.Density() * - 2. / 3. * IGN_PI * r2 * this->radius; - // efunda.com/math/solids/solids_display.cfm?SolidName=Hemisphere - const T ixx = 83. / 320. * hemisphereMass * r2; - const T izz = 2. / 5. * hemisphereMass * r2; - MassMatrix3 hemisphere(hemisphereMass, Vector3(ixx, ixx, izz), - Vector3::Zero);; - - // Distance from centroid of cylinder to centroid of hemisphere, - // since centroid of hemisphere is 3/8 radius from its flat base - const T dz = this->length / 2. + this->radius * 3. / 8.; - Inertial upperCap(hemisphere, Pose3(0, 0, dz, 0, 0, 0)); - Inertial lowerCap(hemisphere, Pose3(0, 0, -dz, 0, 0, 0)); - - // Use Inertial class to add MassMatrix3 objects at different poses - Inertial capsule = - Inertial(cylinder, Pose3::Zero) + upperCap + lowerCap; - - if (!capsule.MassMatrix().IsValid()) - { - return std::nullopt; - } - - return std::make_optional(capsule.MassMatrix()); -} - -////////////////////////////////////////////////// -template -T Capsule::Volume() const -{ - return IGN_PI * std::pow(this->radius, 2) * - (this->length + 4. / 3. * this->radius); -} - -////////////////////////////////////////////////// -template -bool Capsule::SetDensityFromMass(const T _mass) -{ - T newDensity = this->DensityFromMass(_mass); - if (isnan(newDensity)) - return false; - - this->material.SetDensity(newDensity); - return true; -} - -////////////////////////////////////////////////// -template -T Capsule::DensityFromMass(const T _mass) const -{ - if (this->radius <= 0 || this->length <=0 || _mass <= 0) - return std::numeric_limits::quiet_NaN(); - - return _mass / this->Volume(); -} - -} -} -#endif +#include diff --git a/include/ignition/math/detail/Cylinder.hh b/include/ignition/math/detail/Cylinder.hh index ac4a96722..24acee9af 100644 --- a/include/ignition/math/detail/Cylinder.hh +++ b/include/ignition/math/detail/Cylinder.hh @@ -13,137 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_CYLINDER_HH_ -#define IGNITION_MATH_DETAIL_CYLINDER_HH_ -namespace ignition -{ -namespace math -{ + */ -////////////////////////////////////////////////// -template -Cylinder::Cylinder(const T _length, const T _radius, - const Quaternion &_rotOffset) -{ - this->length = _length; - this->radius = _radius; - this->rotOffset = _rotOffset; -} - -////////////////////////////////////////////////// -template -Cylinder::Cylinder(const T _length, const T _radius, - const Material &_mat, const Quaternion &_rotOffset) -{ - this->length = _length; - this->radius = _radius; - this->material = _mat; - this->rotOffset = _rotOffset; -} - -////////////////////////////////////////////////// -template -T Cylinder::Radius() const -{ - return this->radius; -} - -////////////////////////////////////////////////// -template -void Cylinder::SetRadius(const T _radius) -{ - this->radius = _radius; -} - -////////////////////////////////////////////////// -template -T Cylinder::Length() const -{ - return this->length; -} - -////////////////////////////////////////////////// -template -void Cylinder::SetLength(const T _length) -{ - this->length = _length; -} - -////////////////////////////////////////////////// -template -Quaternion Cylinder::RotationalOffset() const -{ - return this->rotOffset; -} - -////////////////////////////////////////////////// -template -void Cylinder::SetRotationalOffset(const Quaternion &_rotOffset) -{ - this->rotOffset = _rotOffset; -} - -////////////////////////////////////////////////// -template -const Material &Cylinder::Mat() const -{ - return this->material; -} - -////////////////////////////////////////////////// -template -void Cylinder::SetMat(const Material &_mat) -{ - this->material = _mat; -} - -////////////////////////////////////////////////// -template -bool Cylinder::operator==(const Cylinder &_cylinder) const -{ - return equal(this->radius, _cylinder.Radius()) && - equal(this->length, _cylinder.Length()) && - this->material == _cylinder.Mat(); -} - -////////////////////////////////////////////////// -template -bool Cylinder::MassMatrix(MassMatrix3d &_massMat) const -{ - return _massMat.SetFromCylinderZ( - this->material, this->length, - this->radius, this->rotOffset); -} - -////////////////////////////////////////////////// -template -T Cylinder::Volume() const -{ - return IGN_PI * std::pow(this->radius, 2) * - this->length; -} - -////////////////////////////////////////////////// -template -bool Cylinder::SetDensityFromMass(const T _mass) -{ - T newDensity = this->DensityFromMass(_mass); - if (newDensity > 0) - this->material.SetDensity(newDensity); - return newDensity > 0; -} - -////////////////////////////////////////////////// -template -T Cylinder::DensityFromMass(const T _mass) const -{ - if (this->radius <= 0 || this->length <=0 || _mass <= 0) - return -1.0; - - return _mass / this->Volume(); -} - -} -} -#endif +#include diff --git a/include/ignition/math/detail/Ellipsoid.hh b/include/ignition/math/detail/Ellipsoid.hh index bc1c16f84..536f49c76 100644 --- a/include/ignition/math/detail/Ellipsoid.hh +++ b/include/ignition/math/detail/Ellipsoid.hh @@ -13,116 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_ELLIPSOID_HH_ -#define IGNITION_MATH_DETAIL_ELLIPSOID_HH_ + */ -#include -#include -#include -#include - -namespace ignition -{ -namespace math -{ - -////////////////////////////////////////////////// -template -Ellipsoid::Ellipsoid(const Vector3 &_radii) : radii(_radii) {} - -////////////////////////////////////////////////// -template -Ellipsoid::Ellipsoid(const Vector3 &_radii, const Material &_mat) -: radii(_radii), material(_mat) {} - -////////////////////////////////////////////////// -template -Vector3 Ellipsoid::Radii() const -{ - return this->radii; -} - -////////////////////////////////////////////////// -template -void Ellipsoid::SetRadii(const Vector3 &_radii) -{ - this->radii = _radii; -} - -////////////////////////////////////////////////// -template -const Material &Ellipsoid::Mat() const -{ - return this->material; -} - -////////////////////////////////////////////////// -template -void Ellipsoid::SetMat(const Material &_mat) -{ - this->material = _mat; -} - -////////////////////////////////////////////////// -template -bool Ellipsoid::operator==(const Ellipsoid &_ellipsoid) const -{ - return this->radii == _ellipsoid.radii && - this->material == _ellipsoid.material; -} - -////////////////////////////////////////////////// -template -std::optional< MassMatrix3 > Ellipsoid::MassMatrix() const -{ - if (this->radii.X() <= 0 || this->radii.Y() <= 0 || this->radii.Z() <= 0) - return std::nullopt; - - // mass and inertia of ellipsoid taken from - // https://en.wikipedia.org/wiki/Ellipsoid - const T mass = this->material.Density() * this->Volume(); - const T x2 = std::pow(this->radii.X(), 2); - const T y2 = std::pow(this->radii.Y(), 2); - const T z2 = std::pow(this->radii.Z(), 2); - const T ixx = (mass / 5.) * (y2 + z2); - const T iyy = (mass / 5.) * (x2 + z2); - const T izz = (mass / 5.) * (x2 + y2); - return std::make_optional>( - mass, Vector3(ixx, iyy, izz), Vector3::Zero); -} - -////////////////////////////////////////////////// -template -T Ellipsoid::Volume() const -{ - const T kFourThirdsPi = 4. * IGN_PI / 3.; - return kFourThirdsPi * this->radii.X() * this->radii.Y() * this->radii.Z(); -} - -////////////////////////////////////////////////// -template -bool Ellipsoid::SetDensityFromMass(const T _mass) -{ - T newDensity = this->DensityFromMass(_mass); - if (isnan(newDensity)) - return false; - - this->material.SetDensity(newDensity); - return true; -} - -////////////////////////////////////////////////// -template -T Ellipsoid::DensityFromMass(const T _mass) const -{ - if (this->radii.X() <= 0 || this->radii.Y() <= 0 || this->radii.Z() <=0 - || _mass <= 0) - return std::numeric_limits::quiet_NaN(); - - return _mass / this->Volume(); -} - -} -} -#endif +#include diff --git a/include/ignition/math/detail/Export.hh b/include/ignition/math/detail/Export.hh new file mode 100644 index 000000000..29fd6d728 --- /dev/null +++ b/include/ignition/math/detail/Export.hh @@ -0,0 +1,18 @@ +/* + * 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 diff --git a/include/ignition/math/detail/Sphere.hh b/include/ignition/math/detail/Sphere.hh index 740ee6741..3a570ccb5 100644 --- a/include/ignition/math/detail/Sphere.hh +++ b/include/ignition/math/detail/Sphere.hh @@ -13,161 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_SPHERE_HH_ -#define IGNITION_MATH_DETAIL_SPHERE_HH_ + */ -#include "ignition/math/Sphere.hh" - -namespace ignition -{ -namespace math -{ -////////////////////////////////////////////////// -template -Sphere::Sphere(const T _radius) -{ - this->radius = _radius; -} - -////////////////////////////////////////////////// -template -Sphere::Sphere(const T _radius, const ignition::math::Material &_mat) -{ - this->radius = _radius; - this->material = _mat; -} - -////////////////////////////////////////////////// -template -T Sphere::Radius() const -{ - return this->radius; -} - -////////////////////////////////////////////////// -template -void Sphere::SetRadius(const T _radius) -{ - this->radius = _radius; -} - -////////////////////////////////////////////////// -template -const ignition::math::Material &Sphere::Material() const -{ - return this->material; -} - -////////////////////////////////////////////////// -template -void Sphere::SetMaterial(const ignition::math::Material &_mat) -{ - this->material = _mat; -} - -////////////////////////////////////////////////// -template -bool Sphere::operator==(const Sphere &_sphere) const -{ - return equal(this->radius, _sphere.Radius()) && - this->material == _sphere.Material(); -} - -////////////////////////////////////////////////// -template -bool Sphere::operator!=(const Sphere &_sphere) const -{ - return !(*this == _sphere); -} - -////////////////////////////////////////////////// -template -bool Sphere::MassMatrix(MassMatrix3d &_massMat) const -{ - return _massMat.SetFromSphere(this->material, this->radius); -} - -////////////////////////////////////////////////// -template -T Sphere::Volume() const -{ - return (4.0/3.0) * IGN_PI * std::pow(this->radius, 3); -} - -////////////////////////////////////////////////// -template -T Sphere::VolumeBelow(const Plane &_plane) const -{ - auto r = this->radius; - // get nearest point to center on plane - auto dist = _plane.Distance(Vector3d(0, 0, 0)); - - if (dist < -r) - { - // sphere is below plane. - return Volume(); - } - else if (dist > r) - { - // sphere is completely above plane - return 0.0; - } - - auto h = r - dist; - return IGN_PI * h * h * (3 * r - h) / 3; -} - -////////////////////////////////////////////////// -template -std::optional> - Sphere::CenterOfVolumeBelow(const Plane &_plane) const -{ - auto r = this->radius; - // get nearest point to center on plane - auto dist = _plane.Distance(Vector3d(0, 0, 0)); - - if (dist < -r) - { - // sphere is completely below plane - return Vector3{0, 0, 0}; - } - else if (dist > r) - { - // sphere is completely above plane - return std::nullopt; - } - - // Get the height of the spherical cap - auto h = r - dist; - - // Formula for geometric centorid: - // https://mathworld.wolfram.com/SphericalCap.html - auto numerator = 2 * r - h; - - auto z = 3 * numerator * numerator / (4 * (3 * r - h)); - return - z * _plane.Normal().Normalized(); -} - -////////////////////////////////////////////////// -template -bool Sphere::SetDensityFromMass(const T _mass) -{ - T newDensity = this->DensityFromMass(_mass); - if (newDensity > 0) - this->material.SetDensity(newDensity); - return newDensity > 0; -} - -////////////////////////////////////////////////// -template -T Sphere::DensityFromMass(const T _mass) const -{ - if (this->radius <= 0 || _mass <= 0) - return -1.0; - - return _mass / this->Volume(); -} -} -} -#endif +#include diff --git a/include/ignition/math/detail/WellOrderedVector.hh b/include/ignition/math/detail/WellOrderedVector.hh index 1040a9bf5..2db4c489c 100644 --- a/include/ignition/math/detail/WellOrderedVector.hh +++ b/include/ignition/math/detail/WellOrderedVector.hh @@ -13,51 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ -#define IGNITION_MATH_DETAIL_WELLORDERED_VECTOR_HH_ -#include + */ -namespace ignition -{ - namespace math - { - /// \brief Comparator for well-ordering vectors. - template - struct WellOrderedVectors - { - /// \brief The normal Vector3::operator< is not actually properly ordered. - /// It does not form an ordinal set. This leads to various complications. - /// To solve this we introduce this function which orders vector3's by - /// their X value first, then their Y values and lastly their Z-values. - /// \param[in] _a - first vector - /// \param[in] _b - second vector - /// \return true if _a comes before _b. - bool operator() (const Vector3& _a, const Vector3& _b) const - { - if (_a[0] < _b[0]) - { - return true; - } - else if (equal(_a[0], _b[0], 1e-3)) - { - if (_a[1] < _b[1]) - { - return true; - } - else if (equal(_a[1], _b[1], 1e-3)) - { - return _a[2] < _b[2]; - } - else - { - return false; - } - } - return false; - } - }; - } -} - -#endif +#include diff --git a/include/ignition/math/graph/Edge.hh b/include/ignition/math/graph/Edge.hh index 94897567a..9353ebf1b 100644 --- a/include/ignition/math/graph/Edge.hh +++ b/include/ignition/math/graph/Edge.hh @@ -13,330 +13,6 @@ * 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 "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. - // cppcheck-suppress noExplicitConstructor - 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(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, - 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, - 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 +#include diff --git a/include/ignition/math/graph/Graph.hh b/include/ignition/math/graph/Graph.hh index 3c7ca10fc..a82f6b960 100644 --- a/include/ignition/math/graph/Graph.hh +++ b/include/ignition/math/graph/Graph.hh @@ -13,798 +13,6 @@ * 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 +#include diff --git a/include/ignition/math/graph/GraphAlgorithms.hh b/include/ignition/math/graph/GraphAlgorithms.hh index 3af10d115..15e68e72a 100644 --- a/include/ignition/math/graph/GraphAlgorithms.hh +++ b/include/ignition/math/graph/GraphAlgorithms.hh @@ -13,348 +13,6 @@ * 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 "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()) - { - // cppcheck-suppress useStlAlgorithm - 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 +#include diff --git a/include/ignition/math/graph/Vertex.hh b/include/ignition/math/graph/Vertex.hh index bc2cd2d83..9c84a0329 100644 --- a/include/ignition/math/graph/Vertex.hh +++ b/include/ignition/math/graph/Vertex.hh @@ -13,140 +13,6 @@ * 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 - -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. - // cppcheck-suppress noExplicitConstructor - 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 +#include